//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "TensorUtils.hpp"
#include <backendsCommon/ITensorHandle.hpp>

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

namespace armnnUtils
{

armnn::TensorShape GetTensorShape(unsigned int numberOfBatches,
                                  unsigned int numberOfChannels,
                                  unsigned int height,
                                  unsigned int width,
                                  const armnn::DataLayout dataLayout)
{
    switch (dataLayout)
    {
        case armnn::DataLayout::NCHW:
            return armnn::TensorShape({numberOfBatches, numberOfChannels, height, width});
        case armnn::DataLayout::NHWC:
            return armnn::TensorShape({numberOfBatches, height, width, numberOfChannels});
        default:
            throw armnn::InvalidArgumentException("Unknown data layout ["
                                                  + std::to_string(static_cast<int>(dataLayout)) +
                                                  "]", CHECK_LOCATION());
    }
}

armnn::TensorInfo GetTensorInfo(unsigned int numberOfBatches,
                                unsigned int numberOfChannels,
                                unsigned int height,
                                unsigned int width,
                                const armnn::DataLayout dataLayout,
                                const armnn::DataType dataType)
{
    switch (dataLayout)
    {
        case armnn::DataLayout::NCHW:
            return armnn::TensorInfo({numberOfBatches, numberOfChannels, height, width}, dataType);
        case armnn::DataLayout::NHWC:
            return armnn::TensorInfo({numberOfBatches, height, width, numberOfChannels}, dataType);
        default:
            throw armnn::InvalidArgumentException("Unknown data layout ["
                                                  + std::to_string(static_cast<int>(dataLayout)) +
                                                  "]", CHECK_LOCATION());
    }
}

std::pair<float, float> FindMinMax(armnn::ITensorHandle* tensorHandle)
{
    auto tensor_data = static_cast<const float *>(tensorHandle->Map(true));
    auto tensor_size = tensorHandle->GetShape().GetNumElements();

    // Set min/max initially to first value in tensor
    float min = tensor_data[0];
    float max = tensor_data[0];

    // Loop over rest of tensor and update min/max if necessary
    for (unsigned int val = 1; val < tensor_size; val++)
    {
        if (tensor_data[val] < min)
        {
            min = tensor_data[val];
        }
        else if (tensor_data[val] > max)
        {
            max = tensor_data[val];
        }
    }

    tensorHandle->Unmap();

    return std::make_pair(min, max);
}

armnn::TensorShape ExpandDims(const armnn::TensorShape& tensorShape, int axis)
{
    unsigned int outputDim = tensorShape.GetNumDimensions() + 1;

    if (axis < -boost::numeric_cast<int>(outputDim) || axis > boost::numeric_cast<int>(tensorShape.GetNumDimensions()))
    {
        throw armnn::InvalidArgumentException(
            boost::str(boost::format("Invalid expansion axis %1% for %2%D input tensor. %3%") %
                       axis %
                       tensorShape.GetNumDimensions() %
                       CHECK_LOCATION().AsString()));
    }

    if (axis < 0)
    {
        axis = boost::numeric_cast<int>(outputDim) + axis;
    }

    std::vector<unsigned int> outputShape;
    for (unsigned int i = 0; i < tensorShape.GetNumDimensions(); ++i)
    {
        outputShape.push_back(tensorShape[i]);
    }
    outputShape.insert(outputShape.begin() + axis, 1);

    return armnn::TensorShape(outputDim, outputShape.data());
}

unsigned int GetNumElementsBetween(const armnn::TensorShape& shape,
                                   const unsigned int firstAxisInclusive,
                                   const unsigned int lastAxisExclusive)
{
    BOOST_ASSERT(0 <= firstAxisInclusive);
    BOOST_ASSERT(firstAxisInclusive <= lastAxisExclusive);
    BOOST_ASSERT(lastAxisExclusive <= shape.GetNumDimensions());
    unsigned int count = 1;
    for (unsigned int i = firstAxisInclusive; i < lastAxisExclusive; i++)
    {
        count *= shape[i];
    }
    return count;
}

unsigned int GetUnsignedAxis(const unsigned int inputDimension, const int axis)
{
    BOOST_ASSERT_MSG(axis < boost::numeric_cast<int>(inputDimension),
                     "Required axis index greater than number of dimensions.");
    BOOST_ASSERT_MSG(axis >= -boost::numeric_cast<int>(inputDimension),
                     "Required axis index lower than negative of the number of dimensions");

    unsigned int uAxis = axis < 0  ?
                         inputDimension - boost::numeric_cast<unsigned int>(abs(axis))
                         : boost::numeric_cast<unsigned int>(axis);
    return uAxis;
}

}


// Copyright 2021 Apex.AI, Inc.
//
// 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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#include <common/types.hpp>
#include <motion_model/linear_motion_model.hpp>
#include <motion_model/stationary_motion_model.hpp>
#include <state_estimation/kalman_filter/kalman_filter.hpp>
#include <state_estimation/measurement/linear_measurement.hpp>
#include <state_estimation/noise_model/wiener_noise.hpp>

#include <gtest/gtest.h>

#include <Eigen/Dense>

#include <tuple>

using autoware::common::state_vector::variable::X;
using autoware::common::state_vector::variable::Y;
using autoware::common::state_vector::variable::YAW;
using autoware::common::state_vector::variable::X_VELOCITY;
using autoware::common::state_vector::variable::Y_VELOCITY;
using autoware::common::state_vector::variable::YAW_CHANGE_RATE;
using autoware::common::state_vector::variable::X_ACCELERATION;
using autoware::common::state_vector::variable::Y_ACCELERATION;
using autoware::common::state_vector::variable::YAW_CHANGE_ACCELERATION;
using autoware::common::state_vector::Variable;
using autoware::common::state_vector::FloatState;
using autoware::common::state_estimation::LinearMeasurement;
using autoware::common::state_estimation::KalmanFilter;
using autoware::common::state_estimation::NoiseInterface;
using autoware::common::state_estimation::WienerNoise;
using autoware::common::state_estimation::make_kalman_filter;
using autoware::common::state_estimation::make_correction_only_kalman_filter;
using autoware::common::motion_model::LinearMotionModel;
using autoware::common::motion_model::StationaryMotionModel;
using autoware::common::state_vector::ConstAccelerationXY32;
using autoware::common::state_vector::ConstAccelerationXYYaw32;
using autoware::common::types::float32_t;

/// @test Test that a filter can be created and reset and is in a valid state throughout this.
TEST(TestKalmanFilter, CreateAndReset) {
  using State = LinearMotionModel<ConstAccelerationXY32>::State;
  using Matrix = State::Matrix;
  LinearMotionModel<ConstAccelerationXY32> motion_model{};
  WienerNoise<ConstAccelerationXY32> noise_model{{1.0F, 1.0F}};
  auto kf = make_kalman_filter(
    motion_model, noise_model, State{}, {{1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F}});
  EXPECT_TRUE(kf.state().vector().isApproxToConstant(0.0F));
  EXPECT_TRUE(kf.covariance().isApprox(Matrix::Identity()));
  kf.reset(State{State::Vector::Ones()}, Matrix::Ones());
  EXPECT_TRUE(kf.state().vector().isApproxToConstant(1.0F));
  EXPECT_TRUE(kf.covariance().isApproxToConstant(1.0F));
}

/// @test Test that predictions without measurements always increase uncertainty.
TEST(TestKalmanFilter, PredictionsIncreaseUncertainty) {
  using State = LinearMotionModel<ConstAccelerationXY32>::State;
  using Matrix = State::Matrix;
  LinearMotionModel<ConstAccelerationXY32> motion_model{};
  WienerNoise<ConstAccelerationXY32> noise_model{{1.0F, 1.0F}};
  auto kf = make_kalman_filter(motion_model, noise_model, State{}, Matrix::Identity());
  EXPECT_TRUE(kf.state().vector().isApproxToConstant(0.0F));
  auto covariance = kf.covariance();
  for (int i = 0; i < 20; ++i) {
    kf.predict(std::chrono::milliseconds{100LL});
    const auto diff = kf.covariance() - covariance;
    EXPECT_TRUE((diff.diagonal().array() > 0.0F).all()) <<
      "New covariance: \n" << kf.covariance() <<
      "\nis not bigger than old one:\n" << covariance;
    covariance = kf.covariance();
  }
}

/// @test Test that we can track a static object measuring its full state.
TEST(TestKalmanFilter, TrackStaticObjectWithDirectMeasurements) {
  using State = LinearMotionModel<ConstAccelerationXY32>::State;
  using Matrix = State::Matrix;
  using MeasurementState = FloatState<
    X, X_VELOCITY, X_ACCELERATION,
    Y, Y_VELOCITY, Y_ACCELERATION>;
  LinearMotionModel<ConstAccelerationXY32> motion_model{};
  WienerNoise<ConstAccelerationXY32> noise_model{{1.0F, 1.0F}};
  auto kf = make_kalman_filter(motion_model, noise_model, State{}, Matrix::Identity());
  EXPECT_TRUE(kf.state().vector().isApproxToConstant(0.0F));
  auto covariance = kf.covariance();
  const MeasurementState::Vector stddev = MeasurementState::Vector::Constant(0.1F);
  for (int i = 0; i < 10; ++i) {
    kf.predict(std::chrono::milliseconds{100LL});
    kf.correct(
      LinearMeasurement<MeasurementState>::create_with_stddev(
        MeasurementState::Vector::Zero(),
        stddev));
    EXPECT_TRUE(kf.state().vector().isApproxToConstant(0.0F)) <<
      "Vector " << kf.state().vector().transpose() << " is not a zero vector.";
    const auto covariance_difference = kf.covariance() - covariance;
    EXPECT_TRUE((covariance_difference.diagonal().array() < 0.0F).all()) <<
      "New covariance: \n" << kf.covariance() <<
      "\nis not smaller than old one:\n" << covariance;
  }
}

/// @test Test that we can track a static object measuring only its partial state.
TEST(TestKalmanFilter, TrackStaticObjectHiddenState) {
  using State = LinearMotionModel<ConstAccelerationXY32>::State;
  using Matrix = State::Matrix;
  using MeasurementState = FloatState<X, Y>;
  LinearMotionModel<ConstAccelerationXY32> motion_model{};
  WienerNoise<ConstAccelerationXY32> noise_model{{1.0F, 1.0F}};
  auto kf = make_kalman_filter(motion_model, noise_model, State{}, 10.0F * Matrix::Identity());
  EXPECT_TRUE(kf.state().vector().isApproxToConstant(0.0F));
  auto covariance = kf.covariance();
  const MeasurementState::Vector stddev = MeasurementState::Vector::Constant(0.1F);
  for (int i = 0; i < 10; ++i) {
    kf.predict(std::chrono::milliseconds{100LL});
    kf.correct(
      LinearMeasurement<MeasurementState>::create_with_stddev(
        MeasurementState::Vector::Zero(),
        stddev));
    EXPECT_TRUE(kf.state().vector().isApproxToConstant(0.0F)) <<
      "Vector " << kf.state().vector().transpose() << " is not a zero vector.";
  }
  // Perform this check only in the end as the covariance of the unobserved variables _can_ grow
  // initially but will eventually fall below the original values.
  const auto covariance_difference = kf.covariance() - covariance;
  EXPECT_TRUE((covariance_difference.diagonal().array() < 0.0F).all()) <<
    "New covariance: \n" << kf.covariance() <<
    "\nis not smaller than old one:\n" << covariance;
}

/// @test Test that we can track a moving object measuring part of its state.
///
/// @details The object is assumed to move at a straight line, changing its orientation with
///     constant angular velocity. All the variables, X, Y, YAW are changing independently.
TEST(TestKalmanFilter, TrackMovingObject) {
  using State = LinearMotionModel<ConstAccelerationXYYaw32>::State;
  using Matrix = State::Matrix;
  using MeasurementState = FloatState<X, Y, YAW>;
  LinearMotionModel<ConstAccelerationXYYaw32> motion_model{};
  WienerNoise<ConstAccelerationXYYaw32> noise_model{{1.0F, 1.0F, 1.0F}};
  const auto initial_covariance = Matrix::Identity();
  auto kf = make_kalman_filter(motion_model, noise_model, State{}, initial_covariance);
  EXPECT_TRUE(kf.state().vector().isApproxToConstant(0.0F));
  const auto speed = 10.0F;  // m/s
  const std::chrono::milliseconds dt{100LL};
  const std::chrono::seconds total_time{5};
  const MeasurementState::Vector stddev = MeasurementState::Vector::Constant(0.1F);
  for (auto t = dt; t <= total_time; t += dt) {
    const auto float_seconds = std::chrono::duration<float32_t>{t}.count();
    const auto travelled_distance = float_seconds * speed;
    const auto observation = travelled_distance * MeasurementState::Vector::Ones();
    kf.predict(std::chrono::milliseconds{100LL});
    kf.correct(
      LinearMeasurement<MeasurementState>::create_with_stddev(observation, stddev));
  }
  const auto total_float_seconds = std::chrono::duration<float32_t>{total_time}.count();
  const float32_t eps = 0.001F;
  EXPECT_NEAR(kf.state().at<X>(), total_float_seconds * speed, eps);
  EXPECT_NEAR(kf.state().at<Y>(), total_float_seconds * speed, eps);
  EXPECT_NEAR(
    kf.state().at<YAW>(),
    autoware::common::helper_functions::wrap_angle(total_float_seconds * speed), eps);
  EXPECT_NEAR(kf.state().at<X_VELOCITY>(), speed, eps);
  EXPECT_NEAR(kf.state().at<Y_VELOCITY>(), speed, eps);
  EXPECT_NEAR(kf.state().at<YAW_CHANGE_RATE>(), speed, eps);
  EXPECT_NEAR(kf.state().at<X_ACCELERATION>(), 0.0F, eps);
  EXPECT_NEAR(kf.state().at<Y_ACCELERATION>(), 0.0F, eps);
  EXPECT_NEAR(kf.state().at<YAW_CHANGE_ACCELERATION>(), 0.0F, eps);
}

/// \test Track a ball thrown at 45 deg angle. We perfectly observe positions of the ball.
///
/// The ball moves at a parabola starting at (0, 0):
///  ^     ___
///  |   _/   \_
///  | _/       \_
///  |/           \_
///  └-------------->
/// start           end
///
TEST(TestKalmanFilter, TrackThrownBall) {
  using namespace std::chrono_literals;
  using FloatSeconds = std::chrono::duration<float32_t>;
  using State = LinearMotionModel<ConstAccelerationXY32>::State;
  using Matrix = State::Matrix;
  using MeasurementState = FloatState<X, Y>;
  LinearMotionModel<ConstAccelerationXY32> motion_model{};
  WienerNoise<ConstAccelerationXY32> noise_model{{1.0F, 1.0F}};

  const float32_t g = -9.80665F;  // m/s^2.
  const float32_t initial_speed = 9.80665F;  // m/s
  State initial_state{};
  initial_state.at<X_VELOCITY>() = initial_speed;
  initial_state.at<Y_VELOCITY>() = initial_speed;
  initial_state.at<Y_ACCELERATION>() = g;
  const auto initial_covariance = Matrix::Identity();
  auto kf = make_kalman_filter(
    motion_model,
    noise_model,
    initial_state,
    initial_covariance);
  // In the way we model the ball it is going to reach the ground at this time.
  const std::chrono::system_clock::time_point start_time{std::chrono::system_clock::now()};
  const auto duration = 2000ms;
  const auto expected_end_time = start_time + duration;

  const auto increment = 10ms;
  const float32_t seconds_increment{FloatSeconds{increment}.count()};
  State expected_state{initial_state};
  const MeasurementState::Vector stddev = MeasurementState::Vector::Constant(0.1F);
  for (auto timestamp = start_time; timestamp <= expected_end_time; timestamp += increment) {
    expected_state.at<X>() += seconds_increment * expected_state.at<X_VELOCITY>();
    expected_state.at<Y>() += seconds_increment * expected_state.at<Y_VELOCITY>();
    expected_state.at<X_VELOCITY>() += seconds_increment * expected_state.at<X_ACCELERATION>();
    expected_state.at<Y_VELOCITY>() += seconds_increment * expected_state.at<Y_ACCELERATION>();

    kf.predict(increment);
    kf.correct(
      LinearMeasurement<MeasurementState>::create_with_stddev(
        MeasurementState::Vector{expected_state.at<X>(), expected_state.at<Y>()},
        stddev));
  }
  // Quickly check our "simulation" of the ball.
  const auto duration_seconds = std::chrono::duration<float32_t>{duration}.count();
  const auto kRelaxedEpsilon = 0.2F;  // Allow up to 20 cm error.
  EXPECT_NEAR(expected_state.at<X>(), initial_speed * duration_seconds, kRelaxedEpsilon);
  EXPECT_NEAR(expected_state.at<Y>(), 0.0F, kRelaxedEpsilon);

  EXPECT_NEAR(expected_state.at<X>(), kf.state().at<X>(), 0.001F);
  EXPECT_NEAR(expected_state.at<Y>(), kf.state().at<Y>(), 0.001F);
  EXPECT_NEAR(expected_state.at<X_VELOCITY>(), kf.state().at<X_VELOCITY>(), kRelaxedEpsilon);
  EXPECT_NEAR(expected_state.at<Y_VELOCITY>(), kf.state().at<Y_VELOCITY>(), kRelaxedEpsilon);
  EXPECT_NEAR(expected_state.at<X_ACCELERATION>(), 0.0F, kRelaxedEpsilon);
  EXPECT_NEAR(expected_state.at<Y_ACCELERATION>(), g, kRelaxedEpsilon);
}

/// \test Check that Kalman Filter can be used for classification. In this example of a traffic
/// light state.
TEST(KalmanFilterWrapperTest, TrafficLightState) {
  struct RED : public Variable {};
  struct GREEN : public Variable {};
  struct ORANGE : public Variable {};

  using TrafficLightState = FloatState<RED, GREEN, ORANGE>;
  const auto uniform_probability = 1.0F / TrafficLightState::size();
  const auto uniform_vector = TrafficLightState::Vector::Constant(uniform_probability);
  TrafficLightState traffic_light_state{uniform_vector};
  EXPECT_FLOAT_EQ(traffic_light_state.at<RED>(), uniform_probability);
  EXPECT_FLOAT_EQ(traffic_light_state.at<GREEN>(), uniform_probability);
  EXPECT_FLOAT_EQ(traffic_light_state.at<ORANGE>(), uniform_probability);

  auto filter = make_correction_only_kalman_filter(
    traffic_light_state, TrafficLightState::Matrix::Identity());

  // Observe a red light.
  const auto red_light_measurement = LinearMeasurement<TrafficLightState>::create_with_stddev(
    {1.0F, 0.0F, 0.0F},
    {0.1F, 0.1F, 0.1F});
  EXPECT_FLOAT_EQ(filter.state().at<RED>(), uniform_probability);
  EXPECT_FLOAT_EQ(filter.state().at<GREEN>(), uniform_probability);
  EXPECT_FLOAT_EQ(filter.state().at<ORANGE>(), uniform_probability);
  filter.correct(red_light_measurement);
  const auto epsilon = 0.1F;  // An epsilon chosen to match the observation covariance.
  EXPECT_GT(filter.state().at<RED>(), uniform_probability + epsilon);
  EXPECT_LT(filter.state().at<GREEN>(), uniform_probability - epsilon);
  EXPECT_LT(filter.state().at<ORANGE>(), uniform_probability - epsilon);

  // Observe a green light.
  const auto green_light_measurement = LinearMeasurement<TrafficLightState>::create_with_stddev(
    {0.0F, 1.0F, 0.0F},
    {0.1F, 0.1F, 0.1F});
  filter.correct(green_light_measurement);
  // We have now seen both green and red once, so it should be roughly 50/50 probability for either
  // of these states of the traffic light, but definitely not orange.
  EXPECT_NEAR(filter.state().at<RED>(), 0.5F, epsilon);
  EXPECT_NEAR(filter.state().at<GREEN>(), 0.5F, epsilon);
  EXPECT_NEAR(filter.state().at<ORANGE>(), 0.0F, epsilon);
}


#include "nodes/NodeSelectSkeleton.h"
#include <boost/assign/list_of.hpp>
#include "Inlet.h"
#include "Outlet.h"
#include "Scene3d.h"
#include "Functions.h"

using namespace hm;

NodeSelectSkeleton::NodeSelectSkeleton(Node::Params params, std::string className)
: NodeUnthreaded(params, className, "Select skeleton", "Choose a skeleton from a scene based on distance.")
, mMode(0)
, mLastDataTimestamp(-42.)
{
	auto p = addParameter("Mode", &mMode, "How to choose the skeleton from the scene. Currently only 'closest' is implemeted");
	p->setEnumerationLabels(boost::assign::list_of("Closest"));

	createInlet(SCENE3D, "Scene to select skeleton from","Input a Scene3d here that you would like to choose a Skeleton3d");
	createOutlet(SKELETON3D, "Selected skeleton", "The Skeleton3d that has been selected from the inputted scene.");
}

NodePtr NodeSelectSkeleton::create(Node::Params params) const
{
	return NodePtr(new NodeSelectSkeleton(params));

}

void NodeSelectSkeleton::step()
{
	assert(mMode == CLOSEST);
	Data data = inlet(0)->dataIfNewerThan(mLastDataTimestamp);
	if (!data.isNull())
	{
		if (data.isScene3d())
		{
			Scene3d scene = data.asScene3d();
			if (!scene.value.empty())
			{
				double closest = std::numeric_limits<double>::max();
				int closestArg = -1;
				for (int i=0; i<scene.value.size(); i++)
				{
					double dist = magnitude(scene.value[i].joint(SPINE_BASE)).value;
					if (dist < closest)
					{
						closest = dist;
						closestArg = i;
					}
					assert(closestArg != -1);
					Data output(scene.value[i]);
					outlet(0)->outputNewData(output);
				}
			}
		}
	}
}


#ifndef vef2vef_ogrpoly_hpp_included_
#define vef2vef_ogrpoly_hpp_included_

#include <string>
#include <vector>

#include <boost/optional.hpp>
#include <boost/filesystem/path.hpp>

#include "math/geometry.hpp"

#include "geo/srsdef.hpp"

using Polygon = math::Polygon;
using Polygons = math::MultiPolygon;

Polygons loadPolygons(const boost::filesystem::path &ogrDataset
                      , const geo::SrsDefinition &srs);

boost::optional<Polygons>
loadPolygons(const boost::optional<boost::filesystem::path> &ogrDataset
             , const geo::SrsDefinition &srs);

Polygons polygonsFromExtents(const math::Extents2 &extents);

boost::optional<Polygons>
polygonsFromExtents(const boost::optional<math::Extents2> &extents);

#endif // vef2vef_ogrpoly_hpp_included_


/*
 * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
 *
 * Copyright 2007-2012 VTT Technical Research Centre of Finland
 *
 * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
 *          <http://www.vtt.fi/multimedia/alvar.html>
 *
 * ALVAR is free software; you can redistribute it and/or modify it under the
 * terms of the GNU Lesser General Public License as published by the Free
 * Software Foundation; either version 2.1 of the License, or (at your option)
 * any later version.
 *
 * This library is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
 * for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with ALVAR; if not, see
 * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
 */

#include <marker_tracking/labelling_method.h>

#include <Eigen/Core>
#include <alvar/Line.h>

marker_tracking::Labeling::Labeling(camera::CameraParameters const& cam) :
  bw_(NULL), cam_(cam) {
  thresh_param1_ = 31;
  thresh_param2_ = 5;
}

marker_tracking::Labeling::~Labeling() {
  if (bw_)
    cvReleaseImage(&bw_);
}

bool marker_tracking::Labeling::CheckBorder(CvSeq* contour, int width, int height) {
  bool ret = true;
  for (int i = 0; i < contour->total; ++i) {
    CvPoint* pt = reinterpret_cast<CvPoint*>(cvGetSeqElem(contour, i));
    if ((pt->x <= 1) || (pt->x >= width-2) || (pt->y <= 1) || (pt->y >= height-2)) ret = false;
  }
  return ret;
}

void marker_tracking::Labeling::SetThreshParams(int param1, int param2) {
  thresh_param1_ = param1;
  thresh_param2_ = param2;
}

marker_tracking::LabelingCvSeq::LabelingCvSeq(camera::CameraParameters const& cam) :
  Labeling(cam), n_blobs_(0), min_edge_(20), min_area_(25) {
  storage_ = cvCreateMemStorage(0);
}

marker_tracking::LabelingCvSeq::~LabelingCvSeq() {
  if (storage_)
    cvReleaseMemStorage(&storage_);
}

void marker_tracking::LabelingCvSeq::LabelSquares(IplImage* image) {
  if (bw_ == NULL) {
    bw_ = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_8U, 1);
  }

  // Convert grayscale and threshold
  assert(image->nChannels == 1);

  cvAdaptiveThreshold(image, bw_, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY_INV, thresh_param1_, thresh_param2_);

  CvSeq* contours;
  CvSeq* squares = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvSeq), storage_);
  CvSeq* square_contours = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvSeq), storage_);

  cvFindContours(bw_, storage_, &contours, sizeof(CvContour),
                 CV_RETR_LIST, CV_CHAIN_APPROX_NONE, cvPoint(0, 0));

  while (contours) {
    if (contours->total < min_edge_) {
      contours = contours->h_next;
      continue;
    }

    CvSeq* result = cvApproxPoly(contours, sizeof(CvContour), storage_,
                                 CV_POLY_APPROX_DP, cvContourPerimeter(contours) * 0.035, 0);

    if ( result->total == 4 && CheckBorder(result, image->width, image->height) &&
         fabs(cvContourArea(result, CV_WHOLE_SEQ)) > min_area_ &&
         cvCheckContourConvexity(result) ) {
      cvSeqPush(squares, result);
      cvSeqPush(square_contours, contours);
    }
    contours = contours->h_next;
  }

  n_blobs_ = squares->total;
  blob_corners.resize(n_blobs_);

  // For every detected 4-corner blob
  for (int i = 0; i < n_blobs_; ++i) {
    std::vector<alvar::Line> fitted_lines(4);
    blob_corners[i].resize(4);
    CvSeq* sq = reinterpret_cast<CvSeq*>(cvGetSeqElem(squares, i));
    CvSeq* square_contour = reinterpret_cast<CvSeq*>(cvGetSeqElem(square_contours, i));

    for (int j = 0; j < 4; ++j) {
      CvPoint* pt0 = reinterpret_cast<CvPoint*>(cvGetSeqElem(sq, j));
      CvPoint* pt1 = reinterpret_cast<CvPoint*>(cvGetSeqElem(sq, (j+1)%4));
      int k0 = -1, k1 = -1;
      for (int k = 0; k < square_contour->total; k++) {
        CvPoint* pt2 = reinterpret_cast<CvPoint*>(cvGetSeqElem(square_contour, k));
        if ((pt0->x == pt2->x) && (pt0->y == pt2->y)) k0=k;
        if ((pt1->x == pt2->x) && (pt1->y == pt2->y)) k1=k;
      }
      int len;
      if (k1 >= k0)
        len = k1 - k0 - 1;  // neither k0 nor k1 are included
      else
        len = square_contour->total - k0 + k1 - 1;
      if (len == 0) len = 1;

      CvMat* line_data = cvCreateMat(1, len, CV_32FC2);
      for (int l = 0; l < len; l++) {
        int ll = (k0 + l + 1) % square_contour->total;
        CvPoint* p = reinterpret_cast<CvPoint*>(cvGetSeqElem(square_contour, ll));
        Eigen::Vector2d undistorted;
        cam_.Convert<camera::DISTORTED, camera::UNDISTORTED>(Eigen::Vector2d(p->x, p->y), &undistorted);
        CvPoint2D32f pp;
        pp.x = undistorted[0];
        pp.y = undistorted[1];

        CV_MAT_ELEM(*line_data, CvPoint2D32f, 0, l) = pp;
      }

      // Fit edge and put to vector of edges
      float params[4] = {0};

      cvFitLine(line_data, CV_DIST_L2, 0, 0.01, 0.01, params);
      fitted_lines[j] = alvar::Line(params);

      cvReleaseMat(&line_data);
    }

    // Calculated four intersection points
    for (size_t j = 0; j < 4; ++j) {
      alvar::PointDouble intc = alvar::Intersection(fitted_lines[j], fitted_lines[(j + 1) % 4]);

      Eigen::Vector2d distorted;
      cam_.Convert<camera::UNDISTORTED, camera::DISTORTED>(Eigen::Vector2d(intc.x, intc.y), &distorted);
      intc.x = distorted[0];
      intc.y = distorted[1];

      // Should we make this always counter-clockwise or clockwise?
      /*
        if (image->origin && j == 1) blob_corners[i][3] = intc;
        else if (image->origin && j == 3) blob_corners[i][1] = intc;
        else blob_corners[i][j] = intc;
      */
      blob_corners[i][j] = intc;
    }
  }

  cvClearMemStorage(storage_);
}


//==================================================================================================
/*!
    @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_ARCH_X86_SSSE3_SIMD_FUNCTION_ABS_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>
#define BOOST_SIMD_ARCH_X86_SSSE3_SIMD_FUNCTION_ABS_HPP_INCLUDED

#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/function/simd/genmask.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;
  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename A0)
                          , bs::ssse3_
                          , bs::pack_<bd::int32_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      return _mm_abs_epi32(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename A0)
                          , bs::ssse3_
                          , bs::pack_<bd::int8_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      return _mm_abs_epi8(a0);
    }
  };
   BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename A0)
                          , bs::ssse3_
                          , bs::pack_<bd::int16_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      return _mm_abs_epi16(a0);
    }
  };

} } }

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2014 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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 BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_AVX_REVERSE_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_AVX_REVERSE_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_AVX_SUPPORT

#include <boost/simd/swar/functions/reverse.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/sdk/simd/meta/retarget.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( reverse_
                                    , boost::simd::tag::avx_
                                    , (A0)
                                    , ((simd_ < double_<A0>
                                              , boost::simd::tag::avx_
                                              >
                                      ))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      return _mm256_permute_pd( _mm256_permute2f128_pd(a0,a0,1), 5 );
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( reverse_
                                    , boost::simd::tag::avx_
                                    , (A0)
                                    , ((simd_ < ints64_<A0>
                                              , boost::simd::tag::avx_
                                              >
                                      ))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename dispatch::meta::as_floating<A0>::type f_t;
      return  bitwise_cast<A0>(reverse(bitwise_cast<f_t>(a0)));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( reverse_
                                    , boost::simd::tag::avx_
                                    , (A0)
                                    , ((simd_ < single_<A0>
                                              , boost::simd::tag::avx_
                                              >
                                      ))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      A0 tmp = _mm256_permute_ps(a0, _MM_SHUFFLE(0,1,2,3) );
      return _mm256_permute2f128_ps(tmp,tmp,1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( reverse_
                                    , boost::simd::tag::avx_
                                    , (A0)
                                    , ((simd_ < ints32_<A0>
                                              , boost::simd::tag::avx_
                                              >
                                      ))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename dispatch::meta::as_floating<A0>::type f_t;
      return  bitwise_cast<A0>(reverse(bitwise_cast<f_t>(a0)));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( reverse_
                                    , boost::simd::tag::avx_
                                    , (A0)
                                    , ((simd_ < arithmetic_<A0>
                                              , boost::simd::tag::avx_
                                              >
                                      ))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename meta::retarget<A0, boost::simd::tag::sse_ >::type  s_t;

      s_t a000 = _mm256_extractf128_si256(a0, 0);
      s_t a011 = _mm256_extractf128_si256(a0, 1);

      return _mm256_insertf128_si256( _mm256_castsi128_si256( reverse(a011) )
                                    , reverse(a000)
                                    , 1
                                    );
    }
  };
} } }

#endif
#endif


#ifndef __GYRO_ROCKET6G_HH__
#define __GYRO_ROCKET6G_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Non-Ideal Gyro Implementation from Rocket6G)
LIBRARY DEPENDENCY:
      ((../../src/gyro/gyro_rocket6g.cpp))
*******************************************************************************/

#include <armadillo>
#include <aux.hh>

#include "gyro/gyro.hh"

class GyroRocket6G : public Gyro {
  TRICK_INTERFACE(GyroRocket6G);

 public:
  GyroRocket6G(Data_exchang &input);
  GyroRocket6G(const GyroRocket6G &other);
  GyroRocket6G &operator=(const GyroRocket6G &other);

  virtual ~GyroRocket6G() {}
  virtual void init(){};
  virtual void algorithm(double int_step);
  virtual int write_to_(const char *bus_name);
  std::default_random_engine generator;

 private:
  /* Routing components */
  VECTOR(EUG, 3); /* *o   (r/s)    Gyro spin axis accel sensitivity */

  VECTOR(EWG, 3); /* *o   (r/s)    Gyro random walk errors */

  VECTOR(EWALKG, 3); /* *o   (r/s0.5) Random walk */

  VECTOR(EUNBG, 3); /* *o   (r)      Gyro cluster misalignment */

  VECTOR(EMISG, 3); /* *o   (r)      Gyro misalignmt */

  VECTOR(ESCALG, 3); /* *o   (--)     Gyro scale factor */

  VECTOR(EBIASG, 3); /* *o   (r/s)    Gyro bias */

  VECTOR(ITA1, 3); /* *o   (--)  Coefficient of gyro noise calculation */

  VECTOR(ITA2, 3); /* *o   (--)  Coefficient of gyro noise calculation */

  VECTOR(BETA, 3); /* *o   (--)  Coefficient of gyro noise calculation */
};

#endif  // __GYRO_ROCKET6G__


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE Speed0SubgraphTest

#include "validate.hpp"
#include <../random_util.hpp>
#include <boost/test/unit_test.hpp>
#include <chrono>
#include <iostream>
#include <vector>

#include "blip.hpp"

using namespace fwtools::subgraph;
using namespace blip;

std::vector<std::unique_ptr<Blip>> getBlips(int taskId, int n_nodes) {
  std::vector<std::unique_ptr<Blip>> blips;
  if (taskId == 0) {
    for (int i = 0; i < n_nodes; ++i) {
      // 0 0 0 0 0 0 ....
      blips.emplace_back(std::unique_ptr<Blip>(new Blip(0, 10.0f, {})));
    }
  }

  else if (taskId == 1) {
    // 0 1 2 3 4 5 6  ... N 0 1 2 3 4 5 6 ... N
    for (int i = 0; i < n_nodes; ++i) {
      blips.emplace_back(
          std::unique_ptr<Blip>(new Blip(i % (n_nodes / 2), 10.0f, {})));
    }
  }

  else if (taskId == 2) {
    // random
    int seed = 1011;
    popart::DefaultRandomEngine eng(seed);
    popart::UniformIntDistribution<int> idis(0, 2);
    for (int i = 0; i < n_nodes; ++i) {
      auto symbol = idis(eng);
      blips.emplace_back(
          std::unique_ptr<Blip>(new Blip(symbol, 10.0f + symbol, {})));
    }
  }

  else if (taskId == 3) {
    // 0 ... 37 0 ... 37 0 ... 37
    for (int i = 0; i < n_nodes; ++i) {
      blips.emplace_back(std::unique_ptr<Blip>(new Blip(i % (37), 10.0f, {})));
    }
  }

  else {
    throw std::runtime_error("Invalid taskId");
  }

  return blips;
}

class AlgoStats {
public:
  std::vector<Match> matches;
  float timeMs;
  int nMatches;
};

AlgoStats getStats(int taskId,
                   int n_nodes,
                   const std::vector<const Blip *> &sched,
                   OutlinerAlgorithm algo) {

  std::vector<std::pair<size_t, size_t>> sequences(sched.size());
  float sequenceBreakCost = 0.0f;

  using namespace std::chrono;

  float threshold = -1.0;
  auto t0         = steady_clock::now();
  auto matches0   = getRinseMatches<const Blip>(
      sched, sequences, threshold, sequenceBreakCost, algo);
  auto t1 = steady_clock::now();
  auto d0 = std::chrono::duration<double, std::milli>(t1 - t0).count();

  AlgoStats stats;
  stats.matches  = matches0;
  stats.timeMs   = d0;
  stats.nMatches = matches0.size();

  return stats;
}

BOOST_AUTO_TEST_CASE(Speed0_Subgraph) {

  auto test = [](OutlinerAlgorithm algo, int n_nodes, int taskId) {
    auto blips = getBlips(taskId, n_nodes);
    std::vector<const Blip *> sched;
    for (int i = 0; i < n_nodes; ++i) {
      sched.push_back(blips[i].get());
    }
    auto stats1 = getStats(taskId, n_nodes, sched, algo);
    std::cout << n_nodes << " Blips : " << stats1.timeMs << " [ms], "
              << stats1.nMatches << " matches." << std::endl;

    BOOST_CHECK(isValid(stats1.matches, sched) == true);
  };

  std::cout << "\nTask 0 : 000000000....000000000" << std::endl;
  std::cout << "-------------------------------" << std::endl;
  std::cout << "Algorithm 0" << std::endl;
  test(OutlinerAlgorithm::ALGO0, 32, 0);
  test(OutlinerAlgorithm::ALGO0, 64, 0);
  test(OutlinerAlgorithm::ALGO0, 128, 0);

  std::cout << "\nAlgorithm 1" << std::endl;
  test(OutlinerAlgorithm::ALGO1, 32, 0);
  test(OutlinerAlgorithm::ALGO1, 64, 0);
  test(OutlinerAlgorithm::ALGO1, 128, 0);
  test(OutlinerAlgorithm::ALGO1, 256, 0);

  std::cout << "\nTask 1 : 0 1 2 ... N 0 1 2 .. N" << std::endl;
  std::cout << "-------------------------------" << std::endl;
  std::cout << "Algorithm 0" << std::endl;
  test(OutlinerAlgorithm::ALGO0, 32, 1);
  test(OutlinerAlgorithm::ALGO0, 64, 1);
  test(OutlinerAlgorithm::ALGO0, 128, 1);

  // Linear : time for Ukkonen to construct the suffix tree
  std::cout << "\nAlgorithm 1" << std::endl;
  test(OutlinerAlgorithm::ALGO1, 512, 1);
  test(OutlinerAlgorithm::ALGO1, 1024, 1);
  test(OutlinerAlgorithm::ALGO1, 2048, 1);
  test(OutlinerAlgorithm::ALGO1, 4096, 1);

  std::cout << "\nTask 2 : uniform random 0/1/2" << std::endl;
  std::cout << "------------------------------" << std::endl;
  std::cout << "Algorithm 0" << std::endl;
  test(OutlinerAlgorithm::ALGO0, 256, 2);
  test(OutlinerAlgorithm::ALGO0, 512, 2);
  test(OutlinerAlgorithm::ALGO0, 1024, 2);

  std::cout << "\nAlgorithm 1" << std::endl;
  test(OutlinerAlgorithm::ALGO1, 256, 2);
  test(OutlinerAlgorithm::ALGO1, 512, 2);
  test(OutlinerAlgorithm::ALGO1, 1024, 2);
  test(OutlinerAlgorithm::ALGO1, 2048, 2);

  std::cout << "\nTask 3 : 0 ... 37 0 ... 37 0 ... 37 etc" << std::endl;
  std::cout << "---------------------------------------" << std::endl;
  std::cout << "Algorithm 0" << std::endl;
  test(OutlinerAlgorithm::ALGO0, 32, 3);
  test(OutlinerAlgorithm::ALGO0, 64, 3);
  test(OutlinerAlgorithm::ALGO0, 128, 3);

  std::cout << "\nAlgorithm 1" << std::endl;
  test(OutlinerAlgorithm::ALGO1, 256, 3);
  test(OutlinerAlgorithm::ALGO1, 512, 3);
  test(OutlinerAlgorithm::ALGO1, 1024, 3);
  test(OutlinerAlgorithm::ALGO1, 2048, 3);
}


/*=========================================================================
  The Software is copyright (c) Commonwealth Scientific and Industrial Research Organisation (CSIRO)
  ABN 41 687 119 230.
  All rights reserved.

  Licensed under the CSIRO BSD 3-Clause License
  You may not use this file except in compliance with the License.
  You may obtain a copy of the License in the file LICENSE.md or at

  https://stash.csiro.au/projects/SMILI/repos/smili/browse/license.txt

  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 "milxQtDiffusionTensorModel.h"

#include <time.h>

#include <vtkMath.h>
#include <vtkLine.h>
#include <vtkSphereSource.h>

#include <boost/math/special_functions/spherical_harmonic.hpp>

milxQtDiffusionTensorModel::milxQtDiffusionTensorModel(QWidget *theParent) : milxQtModel(theParent)
{
    milxQtWindow::prefix = "DTI: ";

    createActions();

    createConnections();
}

milxQtDiffusionTensorModel::~milxQtDiffusionTensorModel()
{
    //dtor
}

void milxQtDiffusionTensorModel::colourByDirection()
{
    vtkSmartPointer<vtkPolyData> currentMesh = model.Result();

    typedef double projectionType;

    ///Determine colours based on axes directions
    vtkSmartPointer<vtkUnsignedCharArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New();
        scalars->SetNumberOfComponents(3);
        scalars->SetNumberOfTuples(currentMesh->GetNumberOfPoints());
        scalars->SetName("Fibre Colours");
        scalars->FillComponent(0, 0);
        scalars->FillComponent(1, 0);
        scalars->FillComponent(2, 0);
    vtkSmartPointer<vtkFloatArray> projections = vtkSmartPointer<vtkFloatArray>::New();
        projections->SetNumberOfComponents(3);
        projections->SetNumberOfTuples(currentMesh->GetNumberOfPoints());
        projections->SetName("Fibre Projections");
        projections->FillComponent(0, 0.0);
        projections->FillComponent(1, 0.0);
        projections->FillComponent(2, 0.0);

    emit working(-1);
    if(currentMesh->GetNumberOfLines() == 0)
    {
        printInfo("No lines in model. Computing colours for mesh.");

        ///Use the dot product in each axis
        for(size_t j = 0; j < 3; j ++)
        {
            projectionType axis[3] = {0.0, 0.0, 0.0};
            axis[j] = 1.0;
            cout << "Computing toward axis " << j << endl;

            ///Colour based on each of the gradient values in that direction
            for(vtkIdType k = 0; k < currentMesh->GetNumberOfPoints(); k ++)
            {
                coordinate currentProjection(projections->GetTuple3(k)), position(currentMesh->GetPoint(k));

                projectionType projection = vtkMath::Dot(axis, position.data_block()); //project to axis being done,
                currentProjection[j] = projection; //projection in each direction

                projections->SetTuple3(k, currentProjection[0], currentProjection[1], currentProjection[2]);
            }
        }

        ///Colour based on each of the gradient values in that direction
        for(vtkIdType k = 0; k < currentMesh->GetNumberOfPoints(); k ++)
        {
            coordinate currentProjection(projections->GetTuple3(k));
            coordinate currentProjSquared = element_product(currentProjection, currentProjection);
            projectionType maxProjection = currentProjSquared.max_value();
            currentProjSquared /= maxProjection;

            unsigned char colourOfPoint[3] = {0, 0, 0};
            colourOfPoint[0] = static_cast<unsigned char>( currentProjSquared[0]*255.0 );
            colourOfPoint[1] = static_cast<unsigned char>( currentProjSquared[1]*255.0 );
            colourOfPoint[2] = static_cast<unsigned char>( currentProjSquared[2]*255.0 );

            scalars->SetTupleValue(k, colourOfPoint);
        }

        currentMesh->GetPointData()->SetVectors(projections);
        currentMesh->GetPointData()->SetScalars(scalars);
    }
    else
    {
        printInfo("Re-colouring lines by axes directions");
        //Ensure lines done properly so can colour appropriately
        currentMesh->GetLines()->InitTraversal();
        vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
        vtkSmartPointer<vtkIdList> idList = vtkSmartPointer<vtkIdList>::New();
        std::vector<vtkIdType> trackLengths;
        for(vtkIdType j = 0; j < currentMesh->GetNumberOfLines(); j ++)
        {
            currentMesh->GetLines()->GetNextCell(idList);
            trackLengths.push_back(idList->GetNumberOfIds());
            for(vtkIdType pointId = 0; pointId < idList->GetNumberOfIds(); pointId ++)
            {
    //            std::cout << idList->GetId(pointId) << " ";

                double position[3];
                currentMesh->GetPoint(idList->GetId(pointId), position);
                points->InsertNextPoint(position);
            }
        }

        //Re-stitch lines together
        vtkIdType step = 0;
        vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
        for(size_t j = 0; j < trackLengths.size(); j ++)
        {
            for(vtkIdType k = step; k < trackLengths[j]-1; k ++)
            {
                vtkSmartPointer<vtkLine> line = vtkSmartPointer<vtkLine>::New();
                    line->GetPointIds()->SetId(0, k);
                    line->GetPointIds()->SetId(1, k+1);
                lines->InsertNextCell(line);
            }
            qDebug() << trackLengths[j] << endl;
            step += trackLengths[j];
        }

        vtkSmartPointer<vtkPolyData> linesPolyData = vtkSmartPointer<vtkPolyData>::New();
            //Add the points to the dataset
            linesPolyData->SetPoints(points);
            //Add the lines to the dataset
            linesPolyData->SetLines(lines);
            linesPolyData->Modified();
        model.SetInput(linesPolyData);
    }

    emit done(-1);
    generateModel();
}

void milxQtDiffusionTensorModel::harmonics(QString ordersString)
{
    bool ok = false;
    if(ordersString.isEmpty())
    {
        ordersString = QInputDialog::getText(this, tr("Enter order magnitudes of harmonics separated by spaces"),
                                            tr("Orders: "), QLineEdit::Normal,
                                            "1 0 0 0 1 1 1 1 1", &ok);
    }
    if (!ok || ordersString.isEmpty())
        return;

    QStringList orders = ordersString.split(" ");
    std::vector<double> sHarmonicCoefficients;
    for(int i = 0; i < orders.size(); i++)
        sHarmonicCoefficients.push_back(orders[i].toDouble());

    int n_coefs = sHarmonicCoefficients.size();
    int l_max   = milxQtDiffusionTensorModel::LforN( n_coefs );

    vtkSmartPointer<vtkSphereSource> sphereSource = vtkSmartPointer<vtkSphereSource>::New();
        sphereSource->SetThetaResolution( 64 );
        sphereSource->SetPhiResolution( 64 );
        sphereSource->SetRadius( 1.0 );
        sphereSource->Update();
    vtkSmartPointer<vtkPolyData> glyph = sphereSource->GetOutput();

    ///For every point in sphere mesh
    double m_x = 0, m_y = 0, m_z = 0;
    for(int i = 0; i < glyph->GetNumberOfPoints(); i++)
    {
        double point_sphere[3];
        glyph->GetPoint(i, point_sphere);
        double x_s = point_sphere[0];
        double y_s = point_sphere[1];
        double z_s = point_sphere[2];

        double x_g, y_g, z_g;

        ///Compute spherical harmonic amplitude
        double amplitude = computeAmplitude( sHarmonicCoefficients, x_s, y_s, z_s, l_max );

        if(amplitude < 0)
            amplitude = 0;

        ///use this to displace sphere points accordingly
        x_g = x_s * amplitude + m_x;
        y_g = y_s * amplitude + m_y;
        z_g = z_s * amplitude + m_z;

        glyph->GetPoints()->SetPoint(i, x_g, y_g, z_g);
    }

    model.SetInput(glyph);
    model.GenerateNormals(true);

    generateModel();
    milxQtRenderWindow::reset();
}

double milxQtDiffusionTensorModel::computeAmplitude(std::vector<double> SH, double x, double y, double z, int lmax)
{
    double az = atan2(y, x);
    double el = acos(z);
    double val = 0.0;
    for (int l = 0; l <= lmax; l+=2)
    {
        // val += SH[milxQtDiffusionTensorModel::index(l,0)] * boost::math::legendre_p<double>(l, 0, z);
        val += SH[milxQtDiffusionTensorModel::index(l,0)] * boost::math::spherical_harmonic_r<double>(l, 0, el,az);
    }

    for (int m = 1; m <= lmax; m++)
    {
//        float caz = cos(m*az);
//        float saz = sin(m*az);
        for (int l = 2*((m+1)/2); l <= lmax; l+=2)
        {
        // float buf = boost::math::legendre_p<double>(l, 0, z);
        // val += SH[index(l,m)]*buf*caz;
        // val += SH[index(l,-m)]*buf*saz;
            val += SH[milxQtDiffusionTensorModel::index(l,m)]*boost::math::spherical_harmonic_r(l,m,el,az);
            val += SH[milxQtDiffusionTensorModel::index(l,-m)]*boost::math::spherical_harmonic_i(l,m,el,az);
        }
    }

    return val;
}

void milxQtDiffusionTensorModel::createActions()
{
    colourDirectionAct = new QAction(this);
        colourDirectionAct->setText(QApplication::translate("Model", "Colour By Direction", 0, QApplication::UnicodeUTF8));
        colourDirectionAct->setShortcut(tr("Alt+c"));
    harmonicsAct = new QAction(this);
        harmonicsAct->setText(QApplication::translate("Model", "Show Spherical Harmonic ...", 0, QApplication::UnicodeUTF8));
        harmonicsAct->setShortcut(tr("Alt+s"));
}

void milxQtDiffusionTensorModel::createConnections()
{
    //Operations
    connect(colourDirectionAct, SIGNAL(triggered()), this, SLOT(colourByDirection()));
    connect(harmonicsAct, SIGNAL(triggered()), this, SLOT(harmonics()));

    //milxQtModel::createConnections();
}

void milxQtDiffusionTensorModel::contextMenuEvent(QContextMenuEvent *currentEvent)
{
    contextMenu = milxQtModel::basicContextMenu(); //!< Only exists for the duration of the context selection

    contextMenu->addSeparator()->setText(tr("DiffusionTensor"));
    contextMenu->addAction(colourDirectionAct);
    contextMenu->addAction(harmonicsAct);
    contextMenu->addSeparator()->setText(tr("Extensions"));
    foreach(QAction *currAct, extActionsToAdd)
    {
        contextMenu->addAction(currAct);
    }
    contextMenu->addSeparator();
    ///Dont display extensions
    contextMenu->addAction(milxQtModel::scaleAct);
    contextMenu->addAction(milxQtRenderWindow::axesAct);
    contextMenu->addAction(milxQtRenderWindow::refreshAct);

    contextMenu->exec(currentEvent->globalPos());
}


#define BOOST_TEST_MAIN TestTransformProducer
#include <boost/test/unit_test.hpp>
#include <stdexcept>
#include <vector>
#include <sam/TupleExpression.hpp>
#include <sam/FeatureMap.hpp>
#include <sam/FeatureSubscriber.hpp>
#include <sam/TransformProducer.hpp>
#include <sam/VastNetflow.hpp>
#include <sam/Identity.hpp>

using namespace sam;

BOOST_AUTO_TEST_CASE( test_transform_producer )
{
  #define DestIp_TimeLapseSeries    1
  #define SrcIp_TimeLapseSeries     2
  #define TimeDiff_TimeLapseSeries  3
  typedef std::tuple<std::size_t, std::string, std::string, double>
    TimeLapseDestSrc;

  auto featureMap = std::make_shared<FeatureMap>();

  std::vector<std::shared_ptr<Expression<VastNetflow>>> expressions;
  std::vector<std::string> names;
  names.push_back("TimeDiff");

  // Expression TimeSeconds - Prev.TimeSeconds
  //
  // TimeSeconds field token
  std::shared_ptr<ExpressionToken<VastNetflow>> fieldToken = std::make_shared<
    FieldToken<TimeSeconds, VastNetflow>>(featureMap);
  // Sub operator token
  std::shared_ptr<ExpressionToken<VastNetflow>> subToken = std::make_shared<
    SubOperator<VastNetflow>>(featureMap);
  // Prev.TimeSeconds
  std::shared_ptr<ExpressionToken<VastNetflow>> prevToken = std::make_shared<
    PrevToken<TimeSeconds, VastNetflow>>(featureMap);

  std::list<std::shared_ptr<ExpressionToken<VastNetflow>>> infixList;
  infixList.push_back(fieldToken);
  infixList.push_back(subToken);
  infixList.push_back(prevToken);

  auto expression = std::make_shared<Expression<VastNetflow>>( infixList );
  expressions.push_back(expression);

  auto tupleExpression =std::make_shared<TupleExpression<VastNetflow>>(expressions);
  size_t nodeId = 0;
  std::string identifier = "destsrc_timelapseseries";

  // queueLength determines how many inputs we see before feeding it out
  // in parallel via BaseProducer::paralleFeed.  We don't want to have to
  // fill up the queue but want immediate response.
  int queueLength = 1;

  auto timeLapseSeries = new TransformProducer<VastNetflow, TimeLapseDestSrc,
                               DestIp, SourceIp>
                              (tupleExpression,
                               nodeId,
                               featureMap,
                               identifier,
                               queueLength);

  // We'll use a feature subscriber and an identity operator to accumulate
  // the timediff values from the TransformProducer.
  int numFeatures = 1;
  std::string outputFile = "outputFileTestTransformProducer.txt";
  auto subscriber = std::make_shared<FeatureSubscriber>(outputFile, numFeatures);

  identifier = "identity";
  auto identity = std::make_shared<Identity<TimeLapseDestSrc,
                    TimeDiff_TimeLapseSeries, DestIp_TimeLapseSeries>>(0,
                    featureMap, "identity");

  // The identity operator gets its input from the timeLapseSeries
  timeLapseSeries->registerConsumer(identity);

  // The subscriber listens to the features produced by the identity operator
  identity->registerSubscriber(subscriber, identifier);

  subscriber->init();

  // We create a bunch or netflows where the only difference is the time, which
  // we increase by 1 second for each netflow.
  std::string before = "1,1,";
  std::string after = ",2013-04-10 08:32:36,"
                     "20130410083236.384094,17,UDP,172.20.2.18,"
                     "239.255.255.250,29986,1900,0,0,0,133,0,1,0,1,0,0";

  int numExamples = 100;
  for (int i = 0; i < numExamples; i++) {

    // Creating the string that represents the netflow and feeding it to
    // the timeLapseSeries.
    std::string s = before + boost::lexical_cast<std::string>(i) + after;
    VastNetflow netflow = makeNetflow(s);
    timeLapseSeries->consume(netflow);
  }

  // Each line should contain the number 1
  auto infile = std::ifstream(outputFile);
  std::string line;
  int i = 0;
  while(std::getline(infile, line)) {
    // The first line is 0 since there wasn't a previous value to work with.
    if (i == 0) {
      BOOST_CHECK_EQUAL(boost::lexical_cast<double>(line), 0.0);
    }
    if (i > 0) {
      BOOST_CHECK_EQUAL(boost::lexical_cast<double>(line), 1.0);
    }
    i++;

  }
  BOOST_CHECK_EQUAL(i, numExamples);
  //std::string result = subscriber->getOutput();
  //boost::char_separator<char> newline("\n");
  //boost::tokenizer<boost::char_separator<char>> newlineTok(result, newline);
  //int i = 0;
  //BOOST_FOREACH(std::string const& line, newlineTok)
  //{
  //}

}


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

BOOST_AUTO_TEST_CASE( edges_test )
{
  edge_iterator eb, ee;
  vertex_iterator vb, ve;

  Surface_fixture f;
  boost::tie(eb, ee) = edges(f.m);
  boost::tie(vb, ve) = vertices(f.m);
  BOOST_CHECK(std::distance(eb, ee) == 7);
  BOOST_CHECK(std::distance(vb, ve) == 5);

  Cube_fixture cf;
  boost::tie(eb, ee) = edges(cf.m);
  boost::tie(vb, ve) = vertices(cf.m);
  BOOST_CHECK(std::distance(eb, ee) == 18);
  BOOST_CHECK(std::distance(vb, ve) == 8);
}

BOOST_AUTO_TEST_CASE( out_edges_test )
{
  Surface_fixture f;

  BOOST_CHECK(out_degree(f.u, f.m) == 2);
  BOOST_CHECK(out_degree(f.v, f.m) == 4);
  std::pair<out_edge_iterator, out_edge_iterator> out = out_edges(f.v, f.m);
  BOOST_CHECK(std::distance(out.first, out.second) == 4);
  out_edge_iterator it = out.first;
  while(it != out.second) {
    // the source should always be u
    BOOST_CHECK(source(*it, f.m) == f.v);
    // the target never
    BOOST_CHECK(target(*it, f.m) != f.v);
   ++it;
  }
}

BOOST_AUTO_TEST_CASE( in_edges_test )
{
  Surface_fixture f;
  BOOST_CHECK(in_degree(f.u, f.m) == 2);
  BOOST_CHECK(in_degree(f.x, f.m) == 3);
  BOOST_CHECK(in_degree(f.v, f.m) == 4);
  std::pair<in_edge_iterator, in_edge_iterator> in = in_edges(f.v, f.m);
  BOOST_CHECK(std::distance(in.first, in.second) == 4);

  in_edge_iterator it = in.first;
  while(it != in.second) {
    // the source should never be u
    BOOST_CHECK(source(*it, f.m) != f.v);
    // the target must always be u
    BOOST_CHECK(target(*it, f.m) == f.v);
    ++it;
  }
}

BOOST_AUTO_TEST_CASE( in_out_equality )
{
  // in and out degrees must be equal for each vertex
  Cube_fixture f;
  for(Sm::Vertex_iterator it = f.m.vertices_begin();
      it != f.m.vertices_end(); ++it) {
    BOOST_CHECK(in_degree(*it, f.m) == out_degree(*it, f.m));
  }
}

BOOST_AUTO_TEST_CASE( face_test )
{
  Surface_fixture f;
  std::pair<enclosure_iterator, enclosure_iterator>
    enc = enclosure(f.f1, f.m);
  BOOST_CHECK(enc.first != enc.second);
  BOOST_CHECK(std::distance(enc.first, enc.second) == 3);
  enclosure_iterator begin = enc.first;
  while(begin != enc.second)
  {
    BOOST_CHECK(face(*begin, f.m) == f.f1);
    ++begin;
  }
}

BOOST_AUTO_TEST_CASE( weight_map_test )
{
  Surface_fixture f;
  Cube_fixture c;

  CGAL::SM_edge_weight_pmap<K> wm1 = boost::get(boost::edge_weight, c.m);
  edge_iterator eb, ee;
  boost::test_tools::check_is_close_t check_close;
  for(boost::tie(eb, ee) = edges(c.m); eb != ee; ++eb) {
    BOOST_CHECK(
      check_close(wm1[*eb], 2.0, boost::test_tools::percent_tolerance_t<double>(0.00001))
      || check_close(wm1[*eb], 2.82843, boost::test_tools::percent_tolerance_t<double>(0.001)));
  }
}

BOOST_AUTO_TEST_CASE( vertices_test )
{
  Surface_fixture f;

  // boost::property_map<Sm,boost::vertex_index_t>::type vi_map = get(f.m, boost::vertex_index);

  // vertex_iterator b,e;

  // for(boost::tie(b,e) = vertices(f.m);
  //     b!= e;
  //     ++b){
  //   std::cout << boost::get(vi_map, *(b)) << std::endl;
  // }

  // boost::property_map<Sm,boost::edge_weight_t>::type ew_map = get(f.m, boost::edge_weight);

  // edge_iterator be, ee;

  // for(boost::tie(be,ee) = edges(f.m);
  //     be!= ee;
  //     ++be){
  //   std::cout << boost::get(ew_map, *(be)) << std::endl;
  // }

}


// C++ standard headers
#include <exception>
#include <string>
#include <math.h>

// Boost headers
#include <boost/shared_ptr.hpp>

// ROS headers
#include <ros/ros.h>
#include <ros/topic.h>

#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/MapMetaData.h>
#include <nav_msgs/Odometry.h>

#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_listener.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>

// OpenCV headers
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

// Hearts headers
#include <hearts_nav_msgs/OccupancyMap.h>
#include <hearts_nav_msgs/GetOccupancyMap.h>

#include <hearts_nav_msgs/OccupancyPOVMap.h>
#include <hearts_nav_msgs/GetOccupancyPOVMap.h>

#include <hearts_nav_msgs/ProjectedView.h>
#include <hearts_nav_msgs/GetProjectedView.h>

#include <hearts_nav_msgs/RobotPose.h>
#include <hearts_nav_msgs/GetRobotPose.h>

#include <hearts_tools/transformations.h>

#define PI 3.14159265
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

static const std::string robotName       = "ROBOT map";
static const std::string povName         = "POV map";
static const std::string odomTopic       = "/mobile_base_controller/odom";
//static const std::string gtodomTopic       = "/ground_truth_odom";

//Occupancy Grid
nav_msgs::OccupancyGrid::ConstPtr map;

//Odometry trajectory
nav_msgs::Odometry::ConstPtr odometry;

//Global parameters
bool debug = false;
int count = 0;

//Server parameters
ros::ServiceClient client_pov, client_map, client_pose;
hearts_nav_msgs::GetProjectedView srv_pov;
hearts_nav_msgs::GetOccupancyMap srv_map;
hearts_nav_msgs::GetRobotPose srv_pose;

//uint32_t *vecmap, *veczoom;

std::string type2str(int type) {
  std::string r;

  uchar depth = type & CV_MAT_DEPTH_MASK;
  uchar chans = 1 + (type >> CV_CN_SHIFT);

  switch ( depth ) {
    case CV_8U:  r = "8U"; break;
    case CV_8S:  r = "8S"; break;
    case CV_16U: r = "16U"; break;
    case CV_16S: r = "16S"; break;
    case CV_32S: r = "32S"; break;
    case CV_32F: r = "32F"; break;
    case CV_64F: r = "64F"; break;
    default:     r = "User"; break;
  }

  r += "C";
  r += (chans+'0');

  return r;
}

void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msgmap)
{
  map = msgmap;
}

bool srvCallback(hearts_nav_msgs::GetOccupancyPOVMap::Request &req, hearts_nav_msgs::GetOccupancyPOVMap::Response &res)
{
	if(!client_pov.call(srv_pov) || !client_map.call(srv_map) || !client_pose.call(srv_pose))
		return false;

	///Get colored occupancy grid map
	//Get occupancy grid map
	if (srv_map.response.occupancy_map.id < 0)
		return false;

	if ( (srv_map.response.occupancy_map.width * srv_map.response.occupancy_map.height) == 0)
		return false;

	cv::Mat occmap;
	int width = srv_map.response.occupancy_map.width;
	int height = srv_map.response.occupancy_map.height;

	Transformations::VectMsg_ToCvMat(srv_map.response.occupancy_map.occmap, width, height, occmap);

	//Color map
	cv::Mat colormap = cv::Mat::zeros(height, width, CV_8UC3);
	cv::Mat pov = cv::Mat::zeros(height, width, CV_8UC3);

	for(int x = 0; x < occmap.cols; x++)
	  for(int y = 0; y < occmap.rows; y++)
	  {
		  if (occmap.at<uchar>(y,x) != 0)
		  {
			  colormap.at<cv::Vec3b>(y,x)[0] = occmap.at<uchar>(y,x);
			  colormap.at<cv::Vec3b>(y,x)[1] = occmap.at<uchar>(y,x);
			  colormap.at<cv::Vec3b>(y,x)[2] = occmap.at<uchar>(y,x);
		  }
	  }

	///Get robot pose in occupancy map coordinates
	cv::Mat o, pos, k, rt;
	double yaw;

	width = srv_pose.response.robot_pose.o_w;
	height = srv_pose.response.robot_pose.o_h;
	Transformations::VectMsg_ToCvMatf(srv_pose.response.robot_pose.o, width, height, o);

	width = srv_pose.response.robot_pose.pos_w;
	height = srv_pose.response.robot_pose.pos_h;
	Transformations::VectMsg_ToCvMatf(srv_pose.response.robot_pose.pos, width, height, pos);

	yaw = srv_pose.response.robot_pose.yaw;

	width = srv_pose.response.robot_pose.k_w;
	height = srv_pose.response.robot_pose.k_h;
	Transformations::VectMsg_ToCvMatf(srv_pose.response.robot_pose.k, width, height, k);

	width = srv_pose.response.robot_pose.rt_w;
	height = srv_pose.response.robot_pose.rt_h;
	Transformations::VectMsg_ToCvMatf(srv_pose.response.robot_pose.rt, width, height, rt);

	///Get POV data
	if ( (srv_pov.response.projected_view.src.width * srv_pov.response.projected_view.src.height) == 0)
	return false;

	pcl::PointCloud<pcl::PointXYZRGBA> pc_pcl;
	pcl::fromROSMsg(srv_pov.response.projected_view.pov, pc_pcl);

	///Map POV data into occupancy grid map
	cv::Mat p = cv::Mat::zeros(3, 1, CV_32FC1);
	cv::Mat r = cv::Mat::zeros(3, 1, CV_32FC1);
	cv::Mat r_ = cv::Mat::zeros(3, 1, CV_32FC1);

	for (int h=0; h<colormap.rows; h++)
	  for (int w=0; w<colormap.cols; w++)
	  {
		  pcl::PointXYZRGBA pt = pc_pcl.at(w, h);
		  if((pt.b > 0)&&(pt.g > 0)&&(pt.r > 0))
		  {
			p.at<float>(0) = -1*pt.x;
			p.at<float>(1) = pt.z;
			p.at<float>(2) = 1;

			/*cv::Mat rt = cv::Mat::zeros(3, 3, CV_32FC1);
			rt.at<float>(0,0) = 1; rt.at<float>(0,1) = 0;
			rt.at<float>(1,0) = 0; rt.at<float>(1,1) = 1;

			rt.at<float>(0,2) = res.at<float>(0)-320;
			rt.at<float>(1,2) = res.at<float>(1)-240;
			rt.at<float>(2,2) = 1;
			r_ = rt*(k*p);*/

			r = rt*(k*p - o) + o;

			//std::cout << "p: " << p << ", r_: " << r_ << ", r: " << r << std::endl;

			int px = int(r.at<float>(0)); int py = int(r.at<float>(1));
			//std::cout << "(" << pt.x << ", " << pt.y << ", " << pt.z
			//		  << ") => (" << px << ", " << py << ")" << std::endl;
			colormap.at<cv::Vec3b>(py,px)[0] = (unsigned char)pt.b;
			colormap.at<cv::Vec3b>(py,px)[1] = (unsigned char)pt.g;
			colormap.at<cv::Vec3b>(py,px)[2] = (unsigned char)pt.r;

			pov.data[h*pov.step + w*3] = (unsigned char)pt.b;
			pov.data[h*pov.step + w*3 + 1] = (unsigned char)pt.g;
			pov.data[h*pov.step + w*3 + 2] = (unsigned char)pt.r;
		  }
	  }

	//std::cout << "rot: " << rot << ", yaw: " << yaw << std::endl;

	//std::cout << "(" << pos.at<float>(0) << ", " << pos.at<float>(1) << ", " << 180*yaw/PI  << ") => ("
	//		  << res.at<float>(0) << ", " << res.at<float>(1) << ")" << std::endl;

	//std::cout << "(" << res.at<float>(0) << ", " << res.at<float>(1) << ") ("
	//		  << res.at<float>(0)-320 << ", " << res.at<float>(1)-240 << ")" << std::endl;

	///Send messages
	std::vector<uint32_t> vecmap;
	int depth = 3;

	Transformations::CvMatC3_ToVectMsg(colormap, &vecmap, width, height);

	res.occupancy_pov_map.id = count++;
	res.occupancy_pov_map.width = width;
	res.occupancy_pov_map.height = height;
	res.occupancy_pov_map.depth = depth;
	res.occupancy_pov_map.occpovmap = vecmap;

	if (debug)
	{
		///Plot current robot pose
		//Get robot position in occupancy coordinates
		cv::Mat kpos = cv::Mat::zeros(3, 1, CV_32FC1);
		kpos = k*pos;

		cv::Point center(kpos.at<float>(0), kpos.at<float>(1));
		int thickness = -1;	int lineType = 8; int radius = 3;
		cv::circle( colormap, center, radius, cv::Scalar( 0, 0, 255 ), thickness, lineType );

		thickness = 1; lineType = 8;
		int length = 4; cv::Point end;
		end.x = length*sin(yaw) + kpos.at<float>(0);
		end.y = length*cos(yaw) + kpos.at<float>(1);
		cv::line( colormap, center, end, cv::Scalar( 0, 255, 0 ), thickness, lineType );

		//cv::Mat zoomcolor = cv::Mat::zeros(480, 640, CV_8UC3);
		//cv::resize(colormap(roi), zoomcolor, zoomcolor.size(), 0, 0, cv::INTER_CUBIC);

		cv::imshow(robotName, colormap);
		//cv::imshow("COLOR zoom", zoomcolor);
		cv::imshow(povName, pov);
		cv::waitKey(15);
	}

	return true;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

// Entry point
int main(int argc, char** argv)
{
  // Init the ROS node
  ros::init(argc, argv, "top_pov2map_srv");

  ROS_INFO("Starting top_pov2map_srv application ...");

  // Precondition: Valid clock
  ros::NodeHandle nh, nh2, nh3, nh4;
  if (!ros::Time::waitForValid(ros::WallDuration(10.0))) // NOTE: Important when using simulated clock
  {
    ROS_FATAL("Timed-out waiting for valid time.");
    return EXIT_FAILURE;
  }

  for (int i = 0; i < argc; i++)
  {
	  if ( strcmp( argv[i], "-debug") == 0 )
		  debug = true;
  }

  // Create the window to show TIAGo's camera images
  if (debug)
  {
	cv::namedWindow(robotName, cv::WINDOW_AUTOSIZE);
	cv::namedWindow(povName, cv::WINDOW_AUTOSIZE);
  }

  ros::ServiceServer service = nh.advertiseService("/brl_hearts/navigation/top_pov2map_srv", srvCallback);

  ROS_INFO_STREAM("Subscribing to /brl_hearts/navigation/mapping_pose_srv ...");
  client_pose = nh2.serviceClient<hearts_nav_msgs::GetRobotPose>("/brl_hearts/navigation/mapping_pose_srv");

  ROS_INFO_STREAM("Subscribing to /brl_hearts/navigation/top_pov_srv ...");
  client_pov = nh3.serviceClient<hearts_nav_msgs::GetProjectedView>("/brl_hearts/navigation/top_pov_srv");

  ROS_INFO_STREAM("Subscribing to /brl_hearts/navigation/top_map_srv ...");
  client_map = nh4.serviceClient<hearts_nav_msgs::GetOccupancyMap>("/brl_hearts/navigation/top_map_srv");

  //enter a loop that processes ROS callbacks. Press CTRL+C to exit the loop
  ros::Rate loop_rate(5);

  ros::spin();

  return EXIT_SUCCESS;
}


#include <iostream>
#include <fstream>
#include <string>
#include <iomanip>
#include <Eigen/Eigen>
#include <glog/logging.h>
#include <experimental/filesystem>
#include <sys/stat.h>
#include <sys/types.h>
#include <dirent.h>
#include <unistd.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>

#include "json.h"
#include "reprojection_error_cost.h"

using json = nlohmann::json;
namespace fs = std::experimental::filesystem;
using namespace lass;

#define print_var(x) std::cout << #x << " " << x << std::endl;

struct FrameData {
    FrameData() {
        K.setIdentity();
        qcw_gt.setIdentity();
        qcw.setIdentity();
        pcw_gt.setZero();
        pcw.setZero();
    }
    Eigen::Quaterniond qcw_gt, qcw;
    Eigen::Vector3d pcw_gt, pcw;
    Eigen::Matrix3d K;
    std::vector<Eigen::Vector3d> landmarks;
    std::vector<Eigen::Vector2d> keypoints;
    std::string filename;
    std::vector<int> inlier_indices;
};

inline std::vector<std::string> get_filenames(std::string base_dir) {
    std::vector<std::string> filenames;
    DIR *dir;
    struct dirent *entry;
    if ((dir = opendir(base_dir.c_str())) != NULL) {
        /* print all the files and directories within directory */
        while ((entry = readdir(dir)) != NULL) {
            // ignore files and hidden folders
            if (entry->d_type == DT_REG && entry->d_name[0] != '.') {
                filenames.emplace_back(entry->d_name);
                // printf("Find files %s\n", entry->d_name);
            }
        }
        closedir(dir);
    }
    return filenames;
}

inline FrameData load_single_from_json(const std::string &filename, std::vector<Eigen::Vector3d> &id2center) {
    std::ifstream ifs(filename);
    json j_data;
    ifs >> j_data;
    ifs.close();
    // std::cout << j_data;
    CHECK(j_data["label"].size() == j_data["uv"].size()) << "size inconsistency";
    FrameData data;
    Eigen::Matrix3d R;
    for (int row = 0; row < 3; row++) {
        for (int col = 0; col < 4; col++) {
            if (col >= 3) {
                data.pcw_gt(row) = j_data["pose"][row * 4 + col];
            } else {
                R(row, col) = j_data["pose"][row * 4 + col];
            }
        }
    }
    data.qcw_gt = R;
    for (int row = 0; row < 3; ++row) {
        for (int col = 0; col < 3; ++col) {
            data.K(row, col) = j_data["camera_k_matrix"][row * 3 + col];
        }
    }
    for (int idx = 0; idx < j_data["label"].size(); idx++) {
        int id = j_data["label"][idx];
        data.landmarks.push_back(id2center[id]);
        data.keypoints.emplace_back(j_data["uv"][idx][0], j_data["uv"][idx][1]);
    }
    data.filename = filename;
    // print_var(data.filename);
    // print_var(data.K);
    // print_var(data.pcw_gt);
    // print_var(data.qcw_gt.coeffs());
    // print_var(data.keypoints.size());
    // print_var(data.landmarks.size());
    return data;
}

inline std::vector<FrameData> load_data_from_json(const std::string &base_dir) {
    std::ifstream cfs(base_dir + "/id2centers.json");
    json j_centers;
    cfs >> j_centers;
    cfs.close();
    std::vector<Eigen::Vector3d> centers(j_centers.size());
    for (int idx = 0; idx < j_centers.size(); idx++) {
        json &j_center = j_centers[idx];
        centers[idx] = {j_center[0], j_center[1], j_center[2]};
    }

    auto filenames = get_filenames(base_dir + "/prediction_json");
    std::sort(filenames.begin(), filenames.end());
    std::vector<FrameData> frames;
    for (const auto &fn : filenames) {
        frames.push_back(load_single_from_json(base_dir + "/prediction_json/" + fn, centers));
    }
    return frames;
}

double compute_reprojection_error(const FrameData &frame) {
    double rpe_sum = 0;
    double count = 0;
    for (const auto &idx : frame.inlier_indices) {
        Eigen::Vector3d pt_c = frame.qcw * frame.landmarks[idx] + frame.pcw;
        Eigen::Vector2d pt_2d = pt_c.hnormalized();
        CHECK(std::abs(frame.K(0, 0) - frame.K(1, 1)) < 1e-5);
        pt_2d = {frame.K(0, 0) * pt_2d.x() + frame.K(0, 2), frame.K(0, 0) * pt_2d.y() + frame.K(1, 2)};
        double rpe = (pt_2d - frame.keypoints[idx]).norm();
        rpe_sum += rpe;
        count++;
    }
    return rpe_sum / count;
}

void solve_pose(FrameData &frame) {
    // get initial pose and inlier points via opencv solvePnPRansac
    std::vector<cv::Point2d> image_pts;
    std::vector<cv::Point3d> world_pts;
    for (int i = 0; i < frame.landmarks.size(); ++i) {
        image_pts.emplace_back(frame.keypoints[i].x(), frame.keypoints[i].y());
        world_pts.emplace_back(frame.landmarks[i].x(), frame.landmarks[i].y(), frame.landmarks[i].z());
    }
    cv::Mat K = cv::Mat::eye(cv::Size(3, 3), CV_64F);
    K.at<double>(0, 0) = frame.K(0, 0);
    K.at<double>(0, 2) = frame.K(0, 2);
    K.at<double>(1, 1) = frame.K(1, 1);
    K.at<double>(1, 2) = frame.K(1, 2);
    cv::Mat rotation, translation;
    std::vector<int> inlier_indices;
    cv::solvePnPRansac(world_pts, image_pts,
                       K, cv::Mat(),
                       rotation, translation, false,
                       200, 8.0, 0.99, inlier_indices,
                       cv::SOLVEPNP_P3P);
    cv::Rodrigues(rotation, rotation);
    Eigen::Matrix3d R;
    cv::cv2eigen(rotation, R);
    frame.qcw = R;
    cv::cv2eigen(translation, frame.pcw);
    // print_var(rotation);
    // print_var(translation);

    // check frustum
    for (auto it = inlier_indices.begin(); it != inlier_indices.end();) {
        Eigen::Vector3d pt_c = frame.qcw * frame.landmarks[*it] + frame.pcw;
        bool is_in_frustum = pt_c.z() > 0;
        Eigen::Vector2d pt_2d = pt_c.hnormalized();
        pt_2d = {frame.K(0, 0) * pt_2d.x() + frame.K(0, 2), frame.K(0, 0) * pt_2d.y() + frame.K(1, 2)};
        if (pt_2d.x() < 0 || pt_2d.x() > 480 || pt_2d.y() < 0 || pt_2d.y() > 270) {
            is_in_frustum = false;
        }
        if (!is_in_frustum) {
            std::cerr << "find pt not in frustum!\n";
            print_var(pt_c);
            print_var(pt_2d);
            it = inlier_indices.erase(it);
        } else {
            ++it;
        }
    }

    // prepare optimize data
    if (inlier_indices.size() < 5) {
        std::cerr << "inliers not enough! current inlier count " << inlier_indices.size() << std::endl;
    }

    frame.inlier_indices = inlier_indices;
    std::vector<Eigen::Vector3d> inlier_landmarks;
    std::vector<Eigen::Vector2d> inlier_keypoints;
    // print_var(inlier_indices.size());
    for (const auto &idx : inlier_indices) {
        inlier_keypoints.push_back(frame.keypoints[idx]);
        inlier_landmarks.push_back(frame.landmarks[idx]);
    }

    // print_var(frame.qcw_gt.coeffs());
    // print_var(frame.qcw.coeffs());
    // print_var(frame.pcw_gt);
    // print_var(frame.pcw);
    // print_var(compute_reprojection_error(frame));
    double rpe_before = compute_reprojection_error(frame);

    // optimize via ceres
    ceres::Problem problem;
    std::array<double, 7> camera = {frame.qcw.w(), frame.qcw.x(), frame.qcw.y(), frame.qcw.z(), frame.pcw.x(), frame.pcw.y(), frame.pcw.z()};
    auto loss_function = new ceres::HuberLoss(1.0);
    ceres::LocalParameterization *camera_parameterization =
        new ceres::ProductParameterization(
            new ceres::QuaternionParameterization(),
            new ceres::IdentityParameterization(3));
    problem.AddParameterBlock(camera.data(), 7, camera_parameterization);
    for (int i = 0; i < inlier_landmarks.size(); ++i) {
        problem.AddParameterBlock(inlier_landmarks[i].data(), 3);
        problem.SetParameterBlockConstant(inlier_landmarks[i].data());
        ceres::CostFunction *cost_function = ReprojectionErrorWithQuaternions::Create(
            inlier_keypoints[i], frame.K);
        problem.AddResidualBlock(cost_function, loss_function, camera.data(), inlier_landmarks[i].data());
    }
    ceres::Solver::Options solver_options;
    solver_options.linear_solver_type = ceres::DENSE_SCHUR;
    solver_options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
    solver_options.use_explicit_schur_complement = true;
    // solver_options.minimizer_progress_to_stdout = true;
    solver_options.minimizer_progress_to_stdout = false;
    ceres::Solver::Summary solver_summary;
    ceres::Solve(solver_options, &problem, &solver_summary);
    // print_var(camera[0]);
    // print_var(camera[1]);
    // print_var(camera[2]);
    // print_var(camera[3]);
    // print_var(camera[4]);
    // print_var(camera[5]);
    // print_var(camera[6]);
    // std::cout << solver_summary.BriefReport();
    // assign valie
    frame.pcw = {camera[4], camera[5], camera[6]};
    frame.qcw = Eigen::Quaterniond(camera[0], camera[1], camera[2], camera[3]);
    double rpe_after = compute_reprojection_error(frame);
    printf("reprojection error : %.4f -> %.4f\n", rpe_before, rpe_after);
}

int main(int argc, char **argv) {
    std::string base_dir = argv[1];
    auto frames = load_data_from_json(base_dir);
    std::vector<double> APEs, AREs;

    for (auto &f : frames) {
        // solve and optimize
        solve_pose(f);
        // evaluate
        if (f.inlier_indices.empty()) {
            APEs.push_back(std::numeric_limits<double>::max());
            AREs.push_back(std::numeric_limits<double>::max());
            continue;
        }
        APEs.push_back((f.pcw - f.pcw_gt).norm() * 1e3);
        Eigen::Quaterniond q_diff = f.qcw * f.qcw_gt.conjugate();
        Eigen::AngleAxisd aa(q_diff);
        AREs.push_back(aa.angle() * 180 / M_PI);
    }
    std::sort(APEs.begin(), APEs.end());
    std::sort(AREs.begin(), AREs.end());
    printf("APE median: %.3f [mm]\nARE median %.3f[DEG]\n", APEs[APEs.size() / 2], AREs[AREs.size() / 2]);
    return 0;
}


/*
 * Copyright (c) 2020, Eberty Alves
 */

// C++ standard library
#include <bits/stdc++.h>

// Point cloud library
#include <pcl/features/normal_3d.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>

// Boost
#include <boost/program_options.hpp>

// Typedefs
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointC;
typedef pcl::PointCloud<pcl::Normal> PointN;

int main(int argc, char* argv[]) {
  try {
    // Declaration of variables
    std::string input_file_name;
    std::string output_file_name;
    uint neighbors;
    bool reverse_normals;
    bool centroid;
    bool origin;

    // Parse command-line options
    namespace po = boost::program_options;

    // Define command-line options
    po::options_description desc("Options");
    desc.add_options()
    ("help,h", "Print help message")
    ("input,i", po::value<std::string>(&input_file_name)->required(), "Input file (.ply)")
    ("output,o", po::value<std::string>(&output_file_name)->required(), "Output file (.ply)")
    ("neighbors,n", po::value<uint>(&neighbors)->default_value(50), "N. of neighbors to analyze for each point")
    ("reverse_normals,r", "Reverse normals' direction")
    ("centroid,c", "Use the centroid as defined view point")
    ("origin,z", "Use the origin as defined view point");

    // Use a parser to evaluate the command line
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);

    // Store the command-line options evaluated by the parser
    if (vm.count("help")) {
      std::cout << "Estimate a set of normals for all the points in the input dataset." << std::endl << std::endl;
      std::cout << desc << std::endl;
      return 0;
    }

    if (vm.count("input") && vm.count("output")) {
      input_file_name = vm["input"].as<std::string>();
      output_file_name = vm["output"].as<std::string>();
    } else {
      throw std::string("Correct mode of use: " + std::string(argv[0]) + " -i input.ply -o output.ply [opts]");
    }

    neighbors = vm["neighbors"].as<uint>();
    reverse_normals = vm.count("reverse_normals");
    centroid = vm.count("centroid");
    origin = vm.count("origin");

    if (origin && centroid) {
      throw std::string("It is not possible to use the centroid and origin as viewpoint at the same time");
    }

    // Load point cloud
    PointC::Ptr cloud(new PointC);
    if (pcl::io::loadPLYFile<PointT>(input_file_name, *cloud) == -1) {
      throw std::string("Couldn't load input file");
    }

    // Create the normal estimation class, and pass the input dataset to it
    pcl::NormalEstimation<PointT, pcl::Normal> ne;
    ne.setInputCloud(cloud);

    // Create an empty kdtree representation, and pass it to the normal estimation object.
    // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
    ne.setSearchMethod(tree);

    // Output datasets
    PointN::Ptr normals(new PointN);

    // Use a certain number of neighbors
    ne.setKSearch(neighbors);

    if (centroid) {
      // Compute 3D centroid
      Eigen::Vector4f centroid;
      pcl::compute3DCentroid(*cloud, centroid);
      ne.setViewPoint(centroid[0], centroid[1], centroid[2]);
    } else if (origin) {
      ne.useSensorOriginAsViewPoint();
    }

    // Compute the normals
    ne.compute(*normals);

    // The setViewPoint function will flip the normals of the whole point cloud.
    // NOTE: Once use the centroid of the set of points as view point, we need to invert the normals to outside the object.
    if ((centroid && !reverse_normals) || (!centroid && reverse_normals)) {
      for (size_t i = 0; i < normals->size(); i++) {
        normals->points[i].normal_x *= -1;
        normals->points[i].normal_y *= -1;
        normals->points[i].normal_z *= -1;
      }
    }

    // Concatenate cloud and normals and save
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
    pcl::concatenateFields(*cloud, *normals, *cloud_normals);
    pcl::io::savePLYFileBinary(output_file_name, *cloud_normals);

    return 0;
  } catch (boost::program_options::error& msg) {
    std::cout << "ERROR: " << msg.what() << std::endl;
  } catch (std::string msg) {
    std::cout << msg << std::endl;
  }

  return -1;
}


//  Boost common_factor.hpp header file  -------------------------------------//

//  (C) Copyright Daryle Walker 2001-2002.
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

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

#ifndef BOOST_INTEGER_COMMON_FACTOR_HPP
#define BOOST_INTEGER_COMMON_FACTOR_HPP

#include <boost/integer/common_factor_ct.hpp>
#include <boost/integer/common_factor_rt.hpp>

#endif  // BOOST_INTEGER_COMMON_FACTOR_HPP


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE test_ordered_trie
#define BOOST_TEST_NO_MAIN

#ifndef BOOST_SYSTEM_NO_DEPRECATED
#define BOOST_SYSTEM_NO_DEPRECATED 1
#endif

#include "test_utils.h"

#include "ordered_trie.hpp"
#include "detail/ordered_trie_node.hpp"
#include "detail/ordered_trie_varint.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/range.hpp>
#include <boost/optional.hpp>

#include <cassert>
#include <iterator>
#include <random>
#include <tuple>
#include <bitset>
#include <unordered_set>
#include <functional>
#include <utility>

BOOST_AUTO_TEST_CASE (dummy) {}

using namespace ordered_trie;
using namespace ordered_trie::test_utils;

namespace
{

template<typename S>
std::vector<typename OrderedTrie<S>::value_type>
make_two_digits_suggestions (size_t length,
			     size_t sample_size,
			     size_t seed)
{
  BOOST_ASSERT (length <= 16);
  BOOST_ASSERT (sample_size <= (1u << (length + 1)));

  std::vector<std::uint64_t>
  suggestion_int ((1 << (length + 1)) - 1, 0);

  std::iota (suggestion_int.begin (),
	     suggestion_int.end (),
	     0u);

  std::shuffle (suggestion_int.begin (),
		suggestion_int.end (),
		std::mt19937_64 {seed});

  suggestion_int.resize (sample_size);

  std::vector<typename OrderedTrie<S>::value_type> result;
  result.reserve (sample_size);
  S score = 0;

  for (const auto v : suggestion_int)
  {
    auto text = std::bitset<17> (v)
     .to_string ()
     .substr (17 - (length + 1));

    text.erase (text.find_last_of ('0'));
    result.push_back ({text, score / 10});
    ++score;
  }

  std::sort (result.begin (), result.end ());
  return result;
}

} // namespace {

BOOST_AUTO_TEST_CASE (test_suggestion_operators)
{
  using Suggestion = OrderedTrie<std::int64_t>::value_type;

  Suggestion foo;
  Suggestion bar {"bar", 0};

  std::set<Suggestion> {foo, bar};

  BOOST_CHECK (bar.string () == "bar");
  BOOST_CHECK (bar.score () == 0);
  BOOST_CHECK (foo < bar);
  BOOST_CHECK (bar == bar);
}

BOOST_AUTO_TEST_CASE (test_serialise_integer)
{
  std::vector<std::uint8_t> output;

  constexpr std::uint8_t a = 0;
  constexpr std::uint8_t b = 255;
  constexpr std::uint16_t c = 0xF00;
  constexpr std::uint16_t d = 0xFF00;
  constexpr std::uint32_t e = 1;
  constexpr std::uint64_t f = std::uint64_t{1} << 40;

  serialise (output, a);
  serialise (output, b);
  serialise (output, c);
  serialise (output, d);
  serialise (output, e);
  serialise (output, f);
  serialise (output, 0.5);

  const std::uint8_t* read_pointer = output.data ();

  BOOST_CHECK_EQUAL (a, deserialise<std::uint8_t> (read_pointer));
  read_pointer = Serialise<std::uint8_t>::skip (read_pointer);

  BOOST_CHECK_EQUAL (b, deserialise<std::uint8_t> (read_pointer));
  read_pointer = Serialise<std::uint8_t>::skip (read_pointer);

  BOOST_CHECK_EQUAL (c, deserialise<std::uint16_t> (read_pointer));
  read_pointer = Serialise<std::uint16_t>::skip (read_pointer);

  BOOST_CHECK_EQUAL (d, deserialise<std::uint16_t> (read_pointer));
  read_pointer = Serialise<std::uint16_t>::skip (read_pointer);

  BOOST_CHECK_EQUAL (e, deserialise<std::uint32_t> (read_pointer));
  read_pointer = Serialise<std::uint32_t>::skip (read_pointer);

  BOOST_CHECK_EQUAL (f, deserialise<std::uint64_t> (read_pointer));
  read_pointer = Serialise<std::uint64_t>::skip (read_pointer);

  BOOST_CHECK_EQUAL (0.5, deserialise<double> (read_pointer));
  read_pointer = Serialise<double>::skip (read_pointer);

  BOOST_CHECK_EQUAL (output.data () + output.size (), read_pointer);
}

BOOST_AUTO_TEST_CASE (test_encoding_32)
{
  using detail::RankEncoder;

  const std::vector<size_t> input =
  {
    0,
    0,
    0x1,
    0xF,
    0x100,
    0x101,
    0x1FF,
    0xFFFF,
    0x100000,
    0x10FFFFF,
    0xFFFFFFFF,
    0x100000000,
    0xABCDEFABCDEFA,
    0x100000000000000,
    0xFFFFFFFFFFFFFFFF
  };

  const std::vector<RankEncoder::wordsize_t> codewords =
  {
    RankEncoder::EMPTY,
    RankEncoder::EMPTY,
    RankEncoder::UINT8,
    RankEncoder::UINT8,
    RankEncoder::UINT16,
    RankEncoder::UINT16,
    RankEncoder::UINT16,
    RankEncoder::UINT16,
    RankEncoder::UINT64,
    RankEncoder::UINT64,
    RankEncoder::UINT64,
    RankEncoder::UINT64,
    RankEncoder::UINT64,
    RankEncoder::UINT64,
    RankEncoder::UINT64,
  };

  std::vector<std::uint8_t> out;

  for (size_t j=0; j < input.size (); ++j)
  {
    BOOST_CHECK_EQUAL (RankEncoder::serialise (out, input[j]),
		       codewords [j]);
  }

  const std::uint8_t *read_offset = out.data ();
  for (size_t j=0; j < input.size (); ++j)
  {

    const auto c = RankEncoder::deserialise (read_offset, codewords [j]);
    read_offset = RankEncoder::skip (read_offset, codewords [j]);
    BOOST_CHECK_EQUAL (c, input [j]);
  }

  BOOST_CHECK_EQUAL (read_offset, out.data () + out.size ());
}

BOOST_AUTO_TEST_CASE (test_encoding_64)
{
  using detail::OffsetEncoder;

  const std::vector<size_t> input =
  {
    0,
    0,
    0x1,
    0xF,
    0x100,
    0x101,
    0x1FF,
    0xFFFF,
    0x100000,
    0x10FFFFF,
    0xFFFFFFFF,
    0x100000000,
    0xFFFFFFFFFFFFFFFF
  };

  const std::vector<OffsetEncoder::wordsize_t> codewords =
  {
    OffsetEncoder::EMPTY,
    OffsetEncoder::EMPTY,
    OffsetEncoder::UINT8,
    OffsetEncoder::UINT8,
    OffsetEncoder::UINT16,
    OffsetEncoder::UINT16,
    OffsetEncoder::UINT16,
    OffsetEncoder::UINT16,
    OffsetEncoder::UINT64,
    OffsetEncoder::UINT64,
    OffsetEncoder::UINT64,
    OffsetEncoder::UINT64,
    OffsetEncoder::UINT64
  };

  std::vector<std::uint8_t> out;

  for (size_t j=0; j < input.size (); ++j)
  {
    BOOST_CHECK_EQUAL (OffsetEncoder::serialise (out, input[j]),
		       codewords [j]);
  }

  const auto* read_offset = out.data ();
  for (size_t j=0; j < codewords.size (); ++j)
  {
    const auto c = OffsetEncoder::deserialise (read_offset, codewords [j]);
    read_offset = OffsetEncoder::skip (read_offset, codewords [j]);

    BOOST_CHECK_EQUAL (c, input [j]);
  }

  BOOST_CHECK_EQUAL (read_offset, out.data () + out.size ());
}

BOOST_AUTO_TEST_CASE (test_node_serialise_internal)
{
  std::vector<std::uint8_t> data;
  detail::serialise_node<Void> (data, "label", 10u, 20u, {});

  const auto node = make_root (data.data ());
  const std::string label {
    node.label_begin (),
    node.label_begin () + node.label_size ()};

  BOOST_CHECK (!node.is_leaf ());
  BOOST_CHECK_EQUAL (label, "label");
  BOOST_CHECK_EQUAL (node.rank (), 10u);
}

BOOST_AUTO_TEST_CASE (test_node_serialise_leaf)
{
  std::vector<std::uint8_t> data;
  detail::serialise_node<Void> (data, "label", 10u, 20u, Void {});

  const auto node = make_root (data.data ());
  const std::string label {
    node.label_begin (),
    node.label_begin () + node.label_size ()};

  BOOST_CHECK (node.is_leaf ());
  BOOST_CHECK_EQUAL (label, "label");
  BOOST_CHECK_EQUAL (node.rank (), 10u);
}

BOOST_AUTO_TEST_CASE (test_ordered_trie_empty)
{
  TemporaryFile tmp_file;
  const auto tmp_path = tmp_file.get ();

  {
    const OrderedTrie<int> trie;
    BOOST_CHECK (trie.empty ());
    BOOST_CHECK (trie.begin () == trie.end ());
    BOOST_CHECK (!trie.count ("x"));
    BOOST_CHECK (!trie.count (""));
    trie.write (tmp_path);
  }

  {
    const auto trie_from_disk =
      OrderedTrie<int>::read (tmp_path);

    BOOST_CHECK (trie_from_disk.empty ());
    BOOST_CHECK (trie_from_disk.begin () == trie_from_disk.end ());
    BOOST_CHECK (!trie_from_disk.count ("x"));
    BOOST_CHECK (!trie_from_disk.count (""));
  }
}

BOOST_AUTO_TEST_CASE (test_ordered_trie_2)
{
  using Suggestion =
    typename OrderedTrie<std::uint64_t>::value_type;

  const OrderedTrie<std::uint64_t> trie
  {
    {"a", 1u}
  };

  const std::vector<Suggestion> suggestions
  {
    {"a", 1u}
  };

  BOOST_ASSERT (trie.count ("a") == 1);
  BOOST_ASSERT (!trie.count (""));
  BOOST_ASSERT (!trie.count ("aa"));
  BOOST_ASSERT (!trie.count ("b"));
  BOOST_ASSERT (trie.score ("a") == 1);
  BOOST_ASSERT (trie.mismatch ("a") == 1);
  BOOST_ASSERT (make_vector(trie) == suggestions);
  BOOST_ASSERT (make_vector(trie.complete("")) ==
		suggestions);
  BOOST_ASSERT (make_vector(trie.complete ("a")) ==
		suggestions);
}

BOOST_AUTO_TEST_CASE (test_ordered_trie_3)
{
  using Suggestion =
    typename OrderedTrie<std::uint64_t>::value_type;

  const std::vector<Suggestion> suggestions =
  {
    {"aaaaaaaaaaa", 30u},
    {"abbb", 1u},
    {"b",    20u},
    {"bcc",  20u},
  };

  const OrderedTrie<std::uint64_t> trie
  {
    suggestions.begin (),
    suggestions.end ()
  };

  BOOST_ASSERT (!trie.count("aa"));
  BOOST_ASSERT (!trie.count(""));
  BOOST_CHECK_EQUAL (trie.count ("b"), 1u);
  BOOST_CHECK_EQUAL (trie.mismatch ("aaaaa"), 5u);
  BOOST_CHECK_EQUAL (trie.mismatch ("aaaaaaaaaaaaa"), 11u);
  BOOST_CHECK_EQUAL (trie.mismatch ("b"), 1u);

  BOOST_CHECK_EQUAL (trie.score ("b"), 20u);
  BOOST_CHECK_EQUAL (trie.score ("bcc"), 20u);
  BOOST_CHECK_EQUAL (trie.score ("b"), 20u);
  BOOST_CHECK_EQUAL (trie.score ("aaaaaaaaaaa"), 30u);

  std::uint64_t s;
  BOOST_CHECK (!trie.score (s, "bccc"));
  BOOST_CHECK (trie.score (s, "bcc"));
  BOOST_CHECK_EQUAL (s, 20u);

  BOOST_ASSERT (
    make_vector(trie) ==
    (std::vector<Suggestion>
    {
      suggestions[0], suggestions[2],
      suggestions[3], suggestions[1]
    }));

  BOOST_ASSERT (
    make_vector(trie.complete("")) ==
    (std::vector<Suggestion>
    {
      suggestions[0], suggestions[2],
      suggestions[3], suggestions[1]
    }));

  BOOST_ASSERT (
    make_vector(trie.complete("a")) ==
    (std::vector<Suggestion> {suggestions[0],
                             suggestions[1]}));

  BOOST_ASSERT (
    make_vector(trie.complete("aaa")) ==
    (std::vector<Suggestion> {suggestions[0]}));

  BOOST_ASSERT (
    make_vector(trie.complete("aaaaaaaaaaa")) ==
    (std::vector<Suggestion> {suggestions[0]}));

  BOOST_ASSERT (
    make_vector(trie.complete("b")) ==
    (std::vector<Suggestion> {suggestions[2],
 	                      suggestions[3]}));

  BOOST_ASSERT (make_vector(trie.complete("d")).empty ());
}

BOOST_AUTO_TEST_CASE (test_ordered_trie_4)
{
  using Suggestion =
    typename OrderedTrie<std::uint64_t>::value_type;

  const std::vector<Suggestion> suggestions =
  {
    {"", 7},
    {"a", 6},
    {"aa", 5},
    {"aaa", 4},
    {"aaaa", 3},
    {"aaaaa", 2},
    {"aaaaaa", 1},
  };

  const auto trie = [&]
  {
    TemporaryFile tmp_file;
    const auto tmp_path = tmp_file.get ();

    OrderedTrie<std::uint64_t>
    {
      suggestions.begin (),
      suggestions.end ()
    }.write (tmp_path);

    return OrderedTrie<std::uint64_t>::read (tmp_path);
  } ();

  BOOST_CHECK_EQUAL (trie.count ("a"), 1u);
  BOOST_CHECK_EQUAL (trie.count ("aaa"), 1u);
  BOOST_CHECK_EQUAL (trie.mismatch ("aa"), 2u);

  BOOST_CHECK (make_vector(trie) == suggestions);
  BOOST_CHECK (
    make_vector (trie.complete ("aaaaa")) ==
    (std::vector<Suggestion>
     {
       suggestions[5], suggestions[6]
     }));
}

BOOST_AUTO_TEST_CASE (test_ordered_trie_5)
{
  using Suggestion =
    typename OrderedTrie<std::uint64_t>::value_type;

  const std::vector<Suggestion> suggestions =
  {
    {"ac", 4u},
    {"ab", 3u},
    {"a",  2u},
    {"ba", 2u},
    {"bd", 1u},
  };

  const auto trie = [&]
  {
    TemporaryFile tmp_file;
    const auto tmp_path = tmp_file.get ();

    make_ordered_trie (suggestions).write (tmp_path);

    return OrderedTrie<std::uint64_t>::read (tmp_path);
  } ();

  BOOST_CHECK (make_vector(trie) == suggestions);

  BOOST_CHECK_EQUAL (trie.count ("a"), 1u);
  BOOST_CHECK_EQUAL (trie.count ("aaa"), 0u);
  BOOST_CHECK_EQUAL (trie.mismatch ("bbb"), 1u);
  BOOST_CHECK (
    make_vector (trie.complete ("a")) ==
    (std::vector<Suggestion>
     {
       suggestions[0],
       suggestions[1],
       suggestions[2]
     }));
}

BOOST_AUTO_TEST_CASE (test_ordered_trie_random_data)
{
  const auto suggestions =
    make_two_digits_suggestions<std::uint64_t> (10, 1000, 63);

  const auto prefixes = [&]
  {
    std::unordered_set<std::string> result;

    for (const auto p : suggestions)
    {
      const auto text = p.first;
      for (size_t j = 0; j <= text.size(); ++j)
      {
	result.insert (text.substr (0, j));
	result.insert (text + "222");
      }
    }

    result.insert ("");
    return result;
  } ();

  const auto check = [&] (OrderedTrie<std::uint64_t> trie)
  {
    for (const auto prefix : prefixes)
    {
      auto result = make_vector (trie.complete (prefix));

      BOOST_CHECK (std::is_sorted (
	result.begin (), result.end (),
	[] (const auto &x, const auto &y)
	{
	  return y.second < x.second;
	}));

      std::sort (result.begin (), result.end ());

      auto expected =
	complete (suggestions, prefix, std::greater<> {});

      std::sort (expected.begin (), expected.end ());

      BOOST_CHECK (result == expected);

      BOOST_CHECK_EQUAL (
	trie.mismatch (prefix),
	mismatch (suggestions, prefix));

      const auto suggestion_it =
	std::lower_bound (
	  suggestions.begin (), suggestions.end (),
	  prefix,
	  [] (const auto &s, const std::string &p)
	  {
	    return std::get<0> (s) < p;
	  });

      if (suggestion_it == suggestions.end () ||
	  suggestion_it->first != prefix)
      {
	BOOST_CHECK_EQUAL (trie.count (prefix), 0u);
	size_t score = -1;
	BOOST_CHECK (!trie.score (score, prefix));
	BOOST_CHECK_EQUAL (score, static_cast<size_t> (-1));
      }
      else
      {
	BOOST_CHECK_EQUAL (trie.count (prefix), 1u);
	size_t score = -1;
	BOOST_CHECK (trie.score (score, prefix));
	BOOST_CHECK_EQUAL (score, suggestion_it->second);
	BOOST_CHECK_EQUAL (
	  trie.score (prefix), suggestion_it->second);
      }
    }
  };

  // Tests different construction patterns

  check (make_ordered_trie (suggestions));

  check (OrderedTrie<std::uint64_t>
  {
    suggestions.begin (),
    suggestions.end ()
  });

  check ([&]
  {
    TemporaryFile tmp_file;
    const auto tmp_path = tmp_file.get ();

    OrderedTrie<std::uint64_t>
    {
      suggestions.begin (),
      suggestions.end ()
    }.write (tmp_path);

    return OrderedTrie<std::uint64_t>::read (tmp_path);
  } ());
}

int main (int argc, char **argv)
{
  return boost::unit_test::unit_test_main(
            &init_unit_test, argc, argv);
}