/**
 * @file  VolumeData.hpp
 * @author Takashi Michikawa <michiawa@acm.org>
 */
#ifndef MI4_VOLUME_DATA_HPP
#define MI4_VOLUME_DATA_HPP 1
#include <cstdlib>
#include <cstring>
#include <fstream>
#include <iostream>
#include <vector>
#include <sstream>
#include <iomanip>

#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace mi4 {
        using Vector3d = Eigen::Vector3d;
        using Vector3i = Eigen::Vector3i;
        using Vector3s = Eigen::Matrix< int16_t, 3, 1 >;
        using Point3d = Eigen::Vector3d;
        using Point3i = Eigen::Vector3i;

        class VolumeInfo {
        private:
                Point3i size_; ///< Global bounding box.
                Point3d pitch_; ///< Voxel pitch.
                Point3d origin_; ///< Origin point. Corresponding to global (0,0,0) in voxel space.
        public:
                explicit VolumeInfo (const Point3i& size = Point3i::Zero(), const Point3d& pitch = Point3d(1, 1, 1), const Point3d& origin = Point3d::Zero()) : size_(size), pitch_(pitch), origin_(origin)
                {
                }

                ~VolumeInfo (void) = default;
                VolumeInfo (const VolumeInfo& that) = default;
                VolumeInfo (VolumeInfo&& info) = default;
                VolumeInfo& operator = (const VolumeInfo& that) = default;
                VolumeInfo& operator = (VolumeInfo&& that) = default;

                VolumeInfo& setSize (const Point3i& size)
                {
                        this->size_ = size;
                        return *this;
                }

                VolumeInfo& setPitch (const Point3d& pitch)
                {
                        this->pitch_ = pitch;
                        return *this;
                }

                VolumeInfo& setOrigin (const Point3d& origin)
                {
                        this->origin_ = origin;
                        return *this;
                }

                VolumeInfo& init (const Point3i& size, const Point3d& pitch, const Point3d& origin)
                {
                        return this->setSize(size).setPitch(pitch).setOrigin(origin);
                }

                VolumeInfo& initByBoundingBox (const Vector3d& bmin, const Vector3d& bmax, const Point3d& pitch, const double offset)
                {
                        const auto off = Eigen::Vector3d::Constant(offset);
                        return this->setOrigin(bmin - off).setPitch(pitch).setSize(this->getPointInVoxelCeil(bmax - bmin + 2 * off));
                }

                Point3i getMin (void) const
                {
                        return Point3i(0, 0, 0);
                }
                Point3i getMax (void) const
                {
                        return this->getSize() - Point3i::Constant(1);
                }
                Point3i getSize (void) const
                {
                        return this->size_;
                }

                Point3d getPitch (void) const
                {
                        return this->pitch_;
                }

                Point3d getOrigin (void) const
                {
                        return this->origin_;
                }

                Point3d getPointInSpace (const Point3i& p) const
                {
                        return this->getPitch().cwiseProduct(p.cast< double >()) + this->getOrigin();
                }

                Point3i getPointInVoxel (const Point3d& p) const
                {
                        return this->getPointInVoxelFloor(p);
                }

                Point3i convertVectorCeil (const Vector3d& p) const
                {
                        return p.cwiseProduct(this->getPitch().cwiseInverse()).unaryExpr([] (double x) { return std::ceil(x); }).cast< int >();
                }

                Point3i convertVectorFloor (const Vector3d& p) const
                {
                        return p.cwiseProduct(this->getPitch().cwiseInverse()).unaryExpr([] (double x) { return std::floor(x); }).cast< int >();
                }

                Point3i getPointInVoxelCeil (const Point3d& p) const
                {
                        return this->convertVectorCeil(p - this->getOrigin());
                }

                Point3i getPointInVoxelFloor (const Point3d& p) const
                {
                        return this->convertVectorFloor(p - this->getOrigin());
                }

                Vector3d getVectorInSpace (const Vector3s& p) const
                {
                        return p.cast< double >().cwiseProduct(this->getPitch()) + this->getOrigin();
                }

                Vector3d getVector (const Vector3i& p) const
                {
                        return this->getPitch().cwiseProduct(p.cast< double >());
                }

                bool isValid (const Point3i& p) const
                {
                        return this->clamp(p) == p;
                }

                Point3i clamp (const Point3i& p) const
                {
                        return p.cwiseMax(this->getMin()).cwiseMin(this->getMax());
                }

                int64_t toIndex (const Point3i& p) const
                {
                        const auto& size = this->getSize();
                        return this->isValid(p) ? static_cast<int64_t> ( p.x()) + static_cast<int64_t> ( size.x()) * static_cast<int64_t> ( p.y() + p.z() * size.y()) : int64_t(-1);
                }

                Point3i fromIndex (const int64_t idx) const
                {
                        const auto& s = this->getSize();
                        return Point3i(static_cast<int> ( idx % s.x()), static_cast<int> ((idx % (s.x() * s.y())) / s.x()), static_cast<int> ( idx / (s.x() * s.y())));
                }

                float getLength (const Point3i& v) const
                {
                        return std::sqrt(this->getLengthSquared(v));
                }

                float getLength (const Vector3s& v) const
                {
                        return std::sqrt(this->getLengthSquared(v.cast< int >()));
                }

                float getLengthSquared (const Point3i& v) const
                {
                        return static_cast<float> ( this->getVector(v).squaredNorm());
                }

                bool isCorner (const Point3i& p) const
                {
                        return this->isValid(p) && (this->getMin().cwiseEqual(p).count() > 0 || this->getMax().cwiseEqual(p).count() > 0);
                }

                VolumeInfo clip (const Point3d& bmin, const Point3d& bmax) const
                {
                        const auto size = this->getPointInVoxelCeil(bmax) - this->getPointInVoxelFloor(bmin) + Point3i(1, 1, 1);
                        return VolumeInfo(size, this->getPitch(), bmin);
                }

                std::string toStringMetaData (void) const
                {
                        auto fn = [] (const auto& p) { return std::to_string(p.x()) + "x" + std::to_string(p.y()) + "x" + std::to_string(p.z()); };
                        return fn(this->getSize()) + "-" + fn(this->getPitch());
                }

                std::string toStringInfo (void) const
                {
                        std::stringstream ss;
                        ss << "size : " << this->getSize().transpose() << ", pitch : " << this->getPitch().transpose() << ", origin : " << this->getOrigin().transpose();
                        return ss.str();
                }
        };

        class Range {
        public:
                class iterator {
                public:
                        explicit iterator (const Range& range, const bool isBegin = true) : range_(range), pos_(range.getMin())
                        {
                                this->pos_.z() = isBegin ? this->pos_.z() : range.getMax().z() + 1;
                        }
                        iterator (const iterator& that) = default;
                        iterator (iterator&& that) = default;
                        iterator& operator = (const iterator& that) = default;
                        iterator& operator = (iterator&& that) = default;
                        virtual ~iterator (void) = default;

                        iterator& operator ++ (void)
                        {
                                this->step_forward();
                                return *this;
                        }

                        bool operator != (const iterator& rhs) const
                        {
                                return this->pos_ != rhs.pos_;
                        }

                        Point3i operator * (void) const
                        {
                                return this->pos_;
                        }

                        iterator& operator += (const size_t n)
                        {
                                for ( size_t i = 0; i < n; ++i ) {
                                        ++(*this);
                                }

                                return *this;
                        }
                private:
                        void step_forward (void)
                        {
                                const auto& bmin = this->range_.getMin();
                                const auto& bmax = this->range_.getMax();
                                auto& pos = this->pos_;

                                if ( pos.z() <= bmax.z()) {
                                        pos.x() += 1;

                                        if ( bmax.x() < pos.x()) {
                                                pos.x() = bmin.x();
                                                pos.y() += 1;

                                                if ( bmax.y() < pos.y()) {
                                                        pos.y() = bmin.y();
                                                        pos.z() += 1;
                                                }
                                        }
                                }
                        }
                private:
                        const Range& range_; // Pointer to the range.
                        Point3i pos_; // Current position.
                };

        public:
                explicit Range (const Point3i& bmin = Point3i(0, 0, 0), const Point3i& bmax = Point3i(0, 0, 0)) : bbox_(Eigen::AlignedBox3i(bmin, bmax))
                {

                }
                explicit Range (const VolumeInfo& info) : bbox_(Eigen::AlignedBox3i(info.getMin(), info.getMax()))
                {

                }
                Range (const Range& range) = default;
                Range& operator = (const Range& range) = default;
                Range (Range&& range) = default;
                Range& operator = (Range&& range) = default;
                ~Range (void) = default;

                Point3i getMin (void) const
                {
                        return this->bbox_.min();
                }

                Point3i getMax (void) const
                {
                        return this->bbox_.max();
                }

                Range::iterator begin (void)
                {
                        return Range::iterator(*this, true);
                }

                Range::iterator end (void)
                {
                        return Range::iterator(*this, false);
                }

        private:
                Eigen::AlignedBox3i bbox_;
        };

        template < typename T >
        class VolumeData {
        public:
                explicit VolumeData (const Point3i& size = Point3i(0, 0, 0), const bool allocateMemory = true)
                {
                        this->init(VolumeInfo(size), allocateMemory);
                }
                explicit VolumeData (const VolumeInfo& info, const bool allocateMemory = true)
                {
                        this->init(info, allocateMemory);
                }
                virtual ~VolumeData (void) = default;

                VolumeData (VolumeData&& that) = default;
                VolumeData& operator = (VolumeData&& that) = default;
                VolumeData (const VolumeData& that) = default;
                VolumeData& operator = (const VolumeData& that) = default;

                VolumeData< T >& init (const VolumeInfo& info, const bool allocateMemory = true)
                {
                        this->data_.clear();
                        this->info_ = info;

                        if ( allocateMemory ) {
                                this->allocate();
                        }

                        return *this;
                }

                VolumeData< T >& fill (const T& value = T())
                {
                        for ( const auto& p : Range(this->getInfo())) {
                                this->at(p) = value;
                        }

                        return *this;
                }

                const VolumeInfo& getInfo (void) const
                {
                        return this->info_;
                }

                Point3i getSize (void) const
                {
                        return this->getInfo().getSize();
                }

                T get (const Point3i& p) const
                {
                        return this->at(p);
                }

                void set (const Point3i& p, const T v)
                {
                        this->at(p) = v;
                }

                T at (const Point3i& p) const
                {
                        return this->at(p.x(), p.y(), p.z());
                }

                T& at (const Point3i& p)
                {
                        return this->at(p.x(), p.y(), p.z());
                }

                T at (const int x, const int y, const int z) const
                {
                        return this->data_.at(z).at(y).at(x);
                }

                T& at (const int x, const int y, const int z)
                {
                        return this->data_.at(z).at(y).at(x);
                }

                bool clone (const VolumeData< T >& that)
                {
                        this->init(that.getInfo(), true);

                        for ( const auto& p : Range(that.getInfo())) {
                                this->at(p) = that.at(p);
                        }

                        return true;
                }

                bool allocate (void)
                {
                        if ( !this->isReadable()) {
                                const auto& size = this->getInfo().getSize();
                                this->data_.assign(size.z(), std::vector< std::vector< T > >(size.y(), std::vector< T >(size.x(), T())));
                                if ( !this->isReadable()) {
                                        this->release();
                                        std::cerr << " error : allocation failed" << std::endl;
                                        return false;
                                }
                        }

                        return true;
                }

                void release (void)
                {
                        this->data_.erase(this->data_.begin(), this->data_.end());
                }

                bool isReadable (void) const
                {
                        const auto& size = this->info_.getSize();
                        if ( this->data_.size() != static_cast<size_t> ( size.z())) return false;
                        for ( const auto& dataxy : this->data_ ) {
                                if ( dataxy.size() != static_cast<size_t> ( size.y())) return false;
                                for ( const auto& datax : dataxy ) {
                                        if ( datax.size() != static_cast<size_t> ( size.x())) return false;
                                }
                        }
                        return true;
                }

                bool open (const std::string& filename, const size_t offset = 0)
                {
                        std::ifstream fin(filename.c_str(), std::ios::binary);
                        return this->read(fin, offset);
                }

                bool save (const std::string& filename)
                {
                        std::ofstream fout(filename.c_str(), std::ios::binary);
                        return this->write(fout);
                }

                bool read (std::ifstream& fin, const size_t offset = 0)
                {
                        if ( !fin ) {
                                std::cerr << " error : file stream is not ready yet." << std::endl;
                                return false;
                        }

                        fin.seekg(offset);

                        for ( auto& dataz : this->data_ ) {
                                for ( auto& datazy : dataz ) {
                                        if ( !fin.read(reinterpret_cast<char *> (datazy.data()), datazy.size() * sizeof(T))) {
                                                return false;
                                        }
                                }
                        }

                        return fin.good();
                }

                bool write (std::ofstream& fout)
                {
                        if ( !this->isReadable()) {
                                std::cerr << "volume data is not readable." << std::endl;
                                return false;
                        } else if ( !fout ) {
                                std::cerr << "the file cannot be open." << std::endl;
                                return false;
                        }

                        for ( const auto& dataz : this->data_ ) {
                                for ( const auto& datazy : dataz ) {
                                        if ( !fout.write(reinterpret_cast<const char *>(datazy.data()), sizeof(T) * datazy.size())) {
                                                return false;
                                        }
                                }
                        }
                        return fout.good();
                }
        private:
                VolumeInfo info_;
                std::vector< std::vector< std::vector< T > > > data_;
        };
}
#endif// MI_VOLUME_DATA_HPP


#include <catch.hpp>
#include <Eigen/Core>
#include <random>

#include "check_adjoint.h"
#include "test_utils.h"
#include "renderer_blending.cuh"

static void testAdjointCamera()
{
	typedef empty TmpStorage_t;
	typedef VectorXr Vector_t;
	static int numSteps = 10;
	static const real3 boxMin = make_real3(-0.2f, -0.3f, -0.4f);
	static const real3 voxelSize = make_real3(0.1f, 0.05f, 0.07f);

	auto forward = [](const Vector_t& x, TmpStorage_t* tmp) -> Vector_t
	{
		const real3 rayStart = fromEigen3(x.segment(0, 3));
		const real3 rayDir = fromEigen3(x.segment(3, 3));
		const real_t stepsize = x[6];
		Vector_t result = Vector_t::Zero(numSteps * 3);
		for (int i=0; i<numSteps; ++i)
		{
			real_t tcurrent = i * stepsize;
			real3 worldPos = rayStart + tcurrent * rayDir;
			real3 volumePos = (worldPos - boxMin) / voxelSize;
			result.segment(3 * i, 3) = toEigen(volumePos);
		}
		return result;
	};
	auto adjoint = [](const Vector_t& x, const Vector_t& e, const Vector_t& g,
		Vector_t& z, const TmpStorage_t& tmp)
	{
		const real3 rayStart = fromEigen3(x.segment(0, 3));
		const real3 rayDir = fromEigen3(x.segment(3, 3));
		const real_t stepsize = x[6];
		real3 adj_rayStart = make_real3(0);
		real3 adj_rayDir = make_real3(0);
		real_t adj_stepSize = 0;
		for (int i=numSteps-1; i>=0; --i)
		{
			//run part of the forward code again
			real_t tcurrent = i * stepsize;
			real3 worldPos = rayStart + tcurrent * rayDir;
			real3 volumePos = (worldPos - boxMin) / voxelSize;

			real3 adj_volumePos = fromEigen3(g.segment(3 * i, 3));

			//adjoint stepping
			real3 adj_worldPos = adj_volumePos / voxelSize;
			adj_rayStart += adj_worldPos;
			adj_rayDir += adj_worldPos * tcurrent;
			real_t adj_tcurrent = dot(adj_worldPos, rayDir);
			adj_stepSize += adj_tcurrent * i;
		}

		z.segment(0, 3) = toEigen(adj_rayStart);
		z.segment(3, 3) = toEigen(adj_rayDir);
		z[6] = adj_stepSize;
	};

	std::default_random_engine rnd(42);
	std::uniform_real_distribution<real_t> distr(0.01, 0.99);
	int N = 20;
	for (int i = 0; i < N; ++i)
	{
		INFO("N=" << i);
		Vector_t x(7);
		for (int j = 0; j < 7; ++j) x[j] = distr(rnd);

		checkAdjoint<Vector_t, TmpStorage_t>(x, forward, adjoint,
			1e-5, 1e-5, 1e-6);
	}
}

TEST_CASE("Adjoint-Camera", "[adjoint]")
{
	testAdjointCamera();
}


#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <utility>
#include <vector>
#include <map>
#include <algorithm>
#include <stdint.h>

#include "gflags/gflags.h"
#include "glog/logging.h"
#include <boost/shared_ptr.hpp>

#include "caffe/proto/caffe.pb.h"
#include "caffe/blob.hpp"
#include "caffe/util/benchmark.hpp"
#include "caffe/util/io.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"
#include "caffe/layer.hpp"
#include "caffe/filler.hpp"

#include "caffe/nhwc/conv_nhwc_layer.hpp"

#include "gtest/gtest.h"
#include "caffe/test/test_caffe_main.hpp"
#include "caffe/test/test_gradient_check_util.hpp"

using namespace std;
using namespace caffe;

int main(int nargc, char** args) {
  // set caffe::GPU
  caffe::Caffe::SetDevice(0);
  caffe::Caffe::set_mode(caffe::Caffe::CPU);

  int channels = 16;  //input
  int num_output = 32;  // output
  int dim = 4; // size
  // define input & output blobs
  Blob<float>* input = new Blob<float>(1, dim, dim, channels);
  Blob<float>* output = new Blob<float>(1, dim, dim, num_output);
  vector<Blob<float>*> bottom_vec;
  vector<Blob<float>*> top_vec;

  // set the bottom blob data
  FillerParameter filler_param;
  filler_param.set_std(0.5);
  GaussianFiller<float> filler(filler_param);
  filler.Fill(input);

  // set the bottom and top data-pointer
  bottom_vec.push_back(input);
  top_vec.push_back(output);

  // create reorg layer
  LayerParameter layer_param;
  layer_param.set_name("ConvNHWC");
  layer_param.set_type("ConvolutionNHWC");
  ConvolutionParameter* conv_param = layer_param.mutable_convolution_param();
  conv_param->set_kernel_h(3);
  conv_param->set_kernel_w(3);
  conv_param->set_stride_h(1);
  conv_param->set_stride_w(1);
  conv_param->set_pad_h(1);
  conv_param->set_pad_w(1);
  conv_param->set_group(1);
  conv_param->set_num_output(num_output);
  conv_param->set_engine(caffe::ConvolutionParameter_Engine_CAFFE);
  FillerParameter* weight_filler = conv_param->mutable_weight_filler();
  weight_filler->set_type("gaussian");
  weight_filler->set_std(0.01);
  FillerParameter* bias_filler = conv_param->mutable_bias_filler();
  bias_filler->set_type("constant");
  bias_filler->set_std(0);

  boost::shared_ptr<Layer<float> > convNHWC_layer = LayerRegistry<float>::CreateLayer(layer_param);
  convNHWC_layer->SetUp(bottom_vec, top_vec);

  convNHWC_layer->Forward(bottom_vec, top_vec);
  // copy diff
  // caffe_copy(top_vec[0]->count(), top_vec[0]->cpu_data(), top_vec[0]->mutable_cpu_diff());
  //
  // for (int i = 0; i < top_vec[0]->count(); ++i) {
  //   LOG(INFO) << top_vec[0]->cpu_data()[i];
  // }
  // LOG(FATAL) << "End.";

  GradientChecker<float> checker(1e-4, 1e-3);
  checker.CheckGradientExhaustive(convNHWC_layer.get(), bottom_vec, top_vec);

  LOG(INFO) << "Gradient Check Finished.";
  return 0;
}


/*********************************************************************
* Rice University Software Distribution License
*
* Copyright (c) 2010, Rice University
* All Rights Reserved.
*
* For a full description see the file named LICENSE.
*
*********************************************************************/

/* Author: Mark Moll */

#include <ompl/tools/benchmark/Benchmark.h>
#include <ompl/control/planners/rrt/RRT.h>
#include <ompl/control/planners/kpiece/KPIECE1.h>
#include <omplapp/apps/KinematicCarPlanning.h>
#include <omplapp/config.h>
#include <boost/math/constants/constants.hpp>

using namespace ompl;

void kinematicCarSetup(app::KinematicCarPlanning& setup)
{
    // plan for kinematic car in SE(2)
    base::StateSpacePtr SE2(setup.getStateSpace());

    // set the bounds for the R^2 part of SE(2)
    base::RealVectorBounds bounds(2);
    bounds.setLow(-10);
    bounds.setHigh(10);
    SE2->as<base::SE2StateSpace>()->setBounds(bounds);

    // define start state
    base::ScopedState<base::SE2StateSpace> start(SE2);
    start->setX(0);
    start->setY(0);
    start->setYaw(0);

    // define goal state
    base::ScopedState<base::SE2StateSpace> goal(SE2);
    goal->setX(2);
    goal->setY(2);
    goal->setYaw(boost::math::constants::pi<double>());

    // set the start & goal states
    setup.setStartAndGoalStates(start, goal, .1);
}

void kinematicCarDemo(app::KinematicCarPlanning& setup)
{
    setup.setPlanner(base::PlannerPtr(new control::KPIECE1(setup.getSpaceInformation())));
    std::vector<double> cs(2);
    cs[0] = cs[1] = 0.1;
    setup.setup();
    setup.getStateSpace()->getDefaultProjection()->setCellSizes(cs);

    // try to solve the problem
    if (setup.solve(20))
    {
        // print the (approximate) solution path: print states along the path
        // and controls required to get from one state to the next
        control::PathControl& path(setup.getSolutionPath());
        //path.interpolate(); // uncomment if you want to plot the path
        path.printAsMatrix(std::cout);
        if (!setup.haveExactSolutionPath())
        {
            std::cout << "Solution is approximate. Distance to actual goal is " <<
                setup.getProblemDefinition()->getSolutionDifference() << std::endl;
        }
    }

}
void kinematicCarBenchmark(app::KinematicCarPlanning& setup)
{
    tools::Benchmark::Request request(20., 10000., 10); // runtime (s), memory (MB), run count

    tools::Benchmark b(setup, setup.getName());
    b.addPlanner(base::PlannerPtr(new control::RRT(setup.getSpaceInformation())));
    b.addPlanner(base::PlannerPtr(new control::KPIECE1(setup.getSpaceInformation())));
    b.benchmark(request);
    b.saveResultsToFile();
}

int main(int argc, char**)
{
    app::KinematicCarPlanning regularCar;

    kinematicCarSetup(regularCar);

    // If any command line arguments are given, solve the problem multiple
    // times with different planners and collect benchmark statistics.
        // Otherwise, solve the problem once for each car type and print the path.
    if (argc>1)
        kinematicCarBenchmark(regularCar);
    else
        kinematicCarDemo(regularCar);
    return 0;
}


#include "Cpp/Classes/BitField.h"
#include "Cpp/Utilities/SuperBitSet.h"
#include "Utilities/ToHexString.h"

#include <boost/test/unit_test.hpp>
#include <sstream> // std::stringstream

using Cpp::Classes::Char4Bits;
using Cpp::Classes::Int4Bits;
using Cpp::Classes::UnsignedChar4Bits;
using Cpp::Classes::UnsignedInt24Bits;
using Cpp::Classes::UnsignedInt4Bits;
using Cpp::Classes::UnsignedInt5Bits;
using Cpp::Utilities::SuperBitSet;
using Utilities::to_hex_string;
using std::stringstream;

BOOST_AUTO_TEST_SUITE(Algorithms)
BOOST_AUTO_TEST_SUITE(Bits)

BOOST_AUTO_TEST_SUITE(BitField_tests)

BOOST_AUTO_TEST_SUITE(Char4Bits_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(CyclesAccordingTo4Bits)
{
  stringstream out;

  Char4Bits a {0};

  BOOST_TEST((a.bits == 0));

  out << to_hex_string(a.bits);

  BOOST_TEST(out.str() == "00");

  a.bits = 1;
  BOOST_TEST((a.bits == 1));

  // Clear the contents of a stringstream.
  // https://stackoverflow.com/questions/20731/how-do-you-clear-a-stringstream-variable
  out.str("");

  out << to_hex_string(a.bits);

  BOOST_TEST(out.str() == "01");

  a.bits = 5;
  BOOST_TEST((a.bits == 5));
  out.str("");
  out << to_hex_string(a.bits);
  BOOST_TEST(out.str() == "05");

  a.bits = 7;
  BOOST_TEST((a.bits == 7));
  out.str("");
  out << to_hex_string(a.bits);
  BOOST_TEST(out.str() == "07");

  a.bits = 8;

  BOOST_TEST((a.bits == -8));
  out.str("");
  out << to_hex_string(a.bits);
  BOOST_TEST(out.str() == "f8");

  a.bits = 9;
  BOOST_TEST((a.bits == -7));
  out.str("");
  out << to_hex_string(a.bits);
  BOOST_TEST(out.str() == "f9");

  a.bits = 14;
  BOOST_TEST((a.bits == -2));
  out.str("");
  out << to_hex_string(a.bits);
  BOOST_TEST(out.str() == "fe");

  a.bits = 15;
  BOOST_TEST((a.bits == -1));
  out.str("");
  out << to_hex_string(a.bits);
  BOOST_TEST(out.str() == "ff");

  // Overflow warning.
  /*
  a.bits = 16;
  BOOST_TEST((a.bits == 0));
  out.str("");
  out << to_hex_string(a.bits);
  BOOST_TEST(out.str() == "00");
  */
}

BOOST_AUTO_TEST_SUITE_END() // Char4Bits_tests

BOOST_AUTO_TEST_SUITE(UnsignedInt4Bits_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(CyclesAccordingTo4Bits)
{
  stringstream out;

  UnsignedInt4Bits a {0};
  BOOST_TEST((a.bits == 0));
  out.str("");
  out << to_hex_string(a.bits);
  BOOST_TEST(out.str() == "00000000");
  BOOST_TEST(SuperBitSet<4>{a.bits}.to_string() == "0000");

  a.bits = 1;
  BOOST_TEST((a.bits == 1));
  out.str("");
  out << to_hex_string(a.bits);
  BOOST_TEST(out.str() == "00000001");
  BOOST_TEST(SuperBitSet<4>{a.bits}.to_string() == "0001");

  a.bits = 2;
  BOOST_TEST((a.bits == 2));
  out.str("");
  out << to_hex_string(a.bits);
  BOOST_TEST(out.str() == "00000002");
  BOOST_TEST(SuperBitSet<4>{a.bits}.to_string() == "0010");

  a.bits = 9;
  BOOST_TEST((a.bits == 9));
  out.str("");
  out << to_hex_string(a.bits);
  BOOST_TEST(out.str() == "00000009");
  BOOST_TEST(SuperBitSet<4>{a.bits}.to_string() == "1001");

  a.bits = 10;
  BOOST_TEST((a.bits == 10));
  out.str("");
  out << to_hex_string(a.bits);
  BOOST_TEST(out.str() == "0000000a");
  BOOST_TEST(SuperBitSet<4>{a.bits}.to_string() == "1010");

  a.bits = 11;
  BOOST_TEST((a.bits == 11));
  out.str("");
  out << to_hex_string(a.bits);
  BOOST_TEST(out.str() == "0000000b");
  BOOST_TEST(SuperBitSet<4>{a.bits}.to_string() == "1011");

  a.bits = 14;
  BOOST_TEST((a.bits == 14));
  out.str("");
  out << to_hex_string(a.bits);
  BOOST_TEST(out.str() == "0000000e");
  BOOST_TEST(SuperBitSet<4>{a.bits}.to_string() == "1110");

  a.bits = 15;
  BOOST_TEST((a.bits == 15));
  out.str("");
  out << to_hex_string(a.bits);
  BOOST_TEST(out.str() == "0000000f");
  BOOST_TEST(SuperBitSet<4>{a.bits}.to_string() == "1111");

  // Overflow warning.
  // a.bits = 16;
}

BOOST_AUTO_TEST_SUITE_END() // UnsignedInt4Bits_tests

BOOST_AUTO_TEST_SUITE(UnsignedInt24Bits_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(CyclesAccordingTo4Bits)
{

}

BOOST_AUTO_TEST_SUITE_END() // UnsignedInt24Bits_tests

BOOST_AUTO_TEST_SUITE_END() // BitField_tests

BOOST_AUTO_TEST_SUITE_END() // Bits
BOOST_AUTO_TEST_SUITE_END() // Algorithms


#include <string>

#define BOOST_TEST_MODULE MixedMIAMergeTests

#include "MIAConfig.h"

#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif
#include "DenseMIA.h"
#include "SparseMIA.h"
constexpr int dim=5;

template<typename data_type>
void do_work(size_t dim1){

    LibMIA::DenseMIA<data_type,3> temp_a(dim1,dim1,dim1);

    LibMIA::DenseMIA<data_type,3> dense_b(dim1,dim1,dim1);
    LibMIA::DenseMIA<data_type,3> dense_c;
    LibMIA::DenseMIA<data_type,3> dense_c2;

    LibMIA::SparseMIA<data_type,3> sparse_a;
    LibMIA::SparseMIA<data_type,3> sparse_c2;

    LibMIA::MIAINDEX i;
    LibMIA::MIAINDEX j;
    LibMIA::MIAINDEX k;

    temp_a.randu(0,20);
    dense_b.randu(0,20);
    for(auto it=temp_a.data_begin();it<temp_a.data_end();++it)
        if(*it<15)
            *it=0;
    for(auto it=dense_b.data_begin();it<dense_b.data_end();++it)
        if(*it<15)
            *it=0;

    sparse_a=temp_a;

    const LibMIA::DenseMIA<data_type,3> dense_a(temp_a); //use a const, just to double check we got const correction right in the expression templates

    dense_c(i,j,k)=dense_a(i,j,k)+dense_b(i,j,k);
    dense_c2(i,j,k)=sparse_a(i,j,k)+dense_b(i,j,k);
    BOOST_CHECK_MESSAGE(dense_c==dense_c2,std::string("Non-destructive Add 1a for ")+typeid(data_type).name());
    //switch order of operands
    dense_c2(i,j,k)=dense_b(i,j,k)+sparse_a(i,j,k);
    BOOST_CHECK_MESSAGE(dense_c==dense_c2,std::string("Non-destructive Add 1b for ")+typeid(data_type).name());

    dense_c(k,i,j)=dense_a(i,k,j)+dense_b(j,k,i);
    dense_c2(k,i,j)=sparse_a(i,k,j)+dense_b(j,k,i);

    BOOST_CHECK_MESSAGE(dense_c==dense_c2,std::string("Non-destructive Add 2a for ")+typeid(data_type).name());
    //switch order of operands
    dense_c2(k,i,j)=dense_b(j,k,i)+sparse_a(i,k,j);
    BOOST_CHECK_MESSAGE(dense_c==dense_c2,std::string("Non-destructive Add 2b for ")+typeid(data_type).name());

    //**Destructive
    dense_c=dense_a;
    sparse_c2=sparse_a;
    dense_c(i,j,k)+=dense_b(i,j,k);
    sparse_c2(i,j,k)+=dense_b(i,j,k);

    BOOST_CHECK_MESSAGE(dense_c==sparse_c2,std::string("Destructive Add 1a for ")+typeid(data_type).name());
    //switch order of operands
    dense_c=dense_b;
    dense_c2=dense_b;
    dense_c(i,j,k)+=dense_a(i,j,k);
    dense_c2(i,j,k)+=sparse_a(i,j,k);
    BOOST_CHECK_MESSAGE(dense_c==dense_c2,std::string("Destructive Add 1b for ")+typeid(data_type).name());

    dense_c=dense_a;
    sparse_c2=sparse_a;
    dense_c(k,i,j)+=dense_b(j,k,i);
    sparse_c2(k,i,j)+=dense_b(j,k,i);

    BOOST_CHECK_MESSAGE(dense_c==sparse_c2,std::string("Destructive Add 2a for ")+typeid(data_type).name());
    //switch order of operands
    dense_c=dense_b;
    dense_c2=dense_b;
    dense_c(k,i,j)+=dense_a(i,k,j);
    dense_c2(k,i,j)+=sparse_a(i,k,j);

    BOOST_CHECK_MESSAGE(dense_c==dense_c2,std::string("Destructive Add 2b for ")+typeid(data_type).name());

    //******Subtract******************

    dense_c(i,j,k)=dense_a(i,j,k)-dense_b(i,j,k);
    dense_c2(i,j,k)=sparse_a(i,j,k)-dense_b(i,j,k);

    BOOST_CHECK_MESSAGE(dense_c==dense_c2,std::string("Non-destructive Sub 1a for ")+typeid(data_type).name());
    //switch order of operands
    dense_c(i,j,k)=dense_b(i,j,k)-dense_a(i,j,k);
    dense_c2(i,j,k)=dense_b(i,j,k)-sparse_a(i,j,k);
    BOOST_CHECK_MESSAGE(dense_c==dense_c2,std::string("Non-destructive Sub 1b for ")+typeid(data_type).name());

    dense_c(k,i,j)=dense_a(i,k,j)-dense_b(j,k,i);
    dense_c2(k,i,j)=sparse_a(i,k,j)-dense_b(j,k,i);

    BOOST_CHECK_MESSAGE(dense_c==dense_c2,std::string("Non-destructive Sub 2a for ")+typeid(data_type).name());
    //switch order of operands
    dense_c(k,i,j)=dense_b(j,k,i)-dense_a(i,k,j);
    dense_c2(k,i,j)=dense_b(j,k,i)-sparse_a(i,k,j);
    BOOST_CHECK_MESSAGE(dense_c==dense_c2,std::string("Non-destructive Sub 2b for ")+typeid(data_type).name());

    //**Destructive
    dense_c=dense_a;
    sparse_c2=sparse_a;
    dense_c(i,j,k)-=dense_b(i,j,k);
    sparse_c2(i,j,k)-=dense_b(i,j,k);

    BOOST_CHECK_MESSAGE(dense_c==sparse_c2,std::string("Destructive Sub 1a for ")+typeid(data_type).name());
    //switch order of operands
    dense_c=dense_b;
    dense_c2=dense_b;
    dense_c(i,j,k)-=dense_a(i,j,k);
    dense_c2(i,j,k)-=sparse_a(i,j,k);
    BOOST_CHECK_MESSAGE(dense_c==dense_c2,std::string("Destructive Sub 1b for ")+typeid(data_type).name());

    dense_c=dense_a;
    sparse_c2=sparse_a;
    dense_c(k,i,j)-=dense_b(j,k,i);
    sparse_c2(k,i,j)-=dense_b(j,k,i);

    BOOST_CHECK_MESSAGE(dense_c==sparse_c2,std::string("Destructive Sub 2a for ")+typeid(data_type).name());
    //switch order of operands
    dense_c=dense_b;
    dense_c2=dense_b;
    dense_c(k,i,j)-=dense_a(i,k,j);
    dense_c2(k,i,j)-=sparse_a(i,k,j);

    BOOST_CHECK_MESSAGE(dense_c==dense_c2,std::string("Destructive Sub 2b for ")+typeid(data_type).name());

}

BOOST_AUTO_TEST_CASE( MixedMIAMergeTests )
{

    size_t dim1=3;

    do_work<float>(dim1);
    do_work<double>(dim1);
    do_work<int32_t>(dim1);
    do_work<int64_t>(dim1);

}


#include <Eigen/Core>
// Some adaptions for gtsam's bundled Eigen being old
#if !(EIGEN_VERSION_AT_LEAST(3, 3, 0))
#define EIGEN_DEVICE_FUNC
namespace Eigen {
using Index = EIGEN_DEFAULT_DENSE_INDEX_TYPE;
}
#endif

#include <benchmark/benchmark.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/slam/expressions.h>
#include "wave/geometry/src/util/math/math.hpp"

template <typename T>
using EigenVector = std::vector<T, Eigen::aligned_allocator<T>>;

using gtsam::Expression;
using gtsam::Point3;
using gtsam::Rot3;

void BM_gtsamAll(benchmark::State &state) {
    const auto N = state.range(0);
    state.SetComplexityN(N);
    // Produce the expression tree
    gtsam::Values values;
    auto p1 = Expression<Point3>{'p', 1};
    auto expr = p1;
    values.insert(gtsam::Symbol{'p', 1}, Point3{Eigen::Vector3d::Random()});

    for (auto i = 0u; i < N; ++i) {
        expr = Expression<Point3>{rotate(Expression<Rot3>{'R', i}, expr)};
        values.insert(gtsam::Symbol{'R', i}, Rot3{wave::randomQuaternion<double>()});
    }
    std::vector<gtsam::Matrix> jacs(N + 1);

    for (auto _ : state) {
        Point3 p2 = expr.value(values, jacs);

        benchmark::DoNotOptimize(p2);
        benchmark::DoNotOptimize(jacs);
    }
}

// This benchmark tests gtsam's valueAndJacobianMap() function, which is normally private
#ifdef LK_CUSTOM_GTSAM_PUBLIC
void BM_gtsamPrivate(benchmark::State &state) {
    const auto N = state.range(0);
    state.SetComplexityN(N);
    // Produce the expression tree
    gtsam::Values values;
    auto p1 = Expression<Point3>{'p', 1};
    auto expr = p1;
    values.insert(gtsam::Symbol{'p', 1}, Point3{Eigen::Vector3d::Random()});

    for (auto i = 0u; i < N; ++i) {
        expr = Expression<Point3>{rotate(Expression<Rot3>{'R', i}, expr)};
        values.insert(gtsam::Symbol{'R', i}, Rot3{wave::randomQuaternion<double>()});
    }
    std::vector<gtsam::Matrix> jacs(N + 1);

    for (auto _ : state) {
        // Pre-allocate and zero VerticalBlockMatrix
        gtsam::KeyVector keys;
        gtsam::FastVector<int> dims;
        boost::tie(keys, dims) = expr.keysAndDims();
        static const int Dim = gtsam::traits<Point3>::dimension;
        gtsam::VerticalBlockMatrix Ab(dims, Dim);
        Ab.matrix().setZero();
        gtsam::internal::JacobianMap jacobianMap(keys, Ab);

        // Call unsafe version
        Point3 p2 = expr.valueAndJacobianMap(values, jacobianMap);

        benchmark::DoNotOptimize(p2);
        benchmark::DoNotOptimize(values);
        benchmark::DoNotOptimize(jacobianMap);
    }
}
BENCHMARK(BM_gtsamPrivate)->RangeMultiplier(2)->Range(1, 1 << 14)->Complexity();
#endif

BENCHMARK(BM_gtsamAll)->RangeMultiplier(2)->Range(1, 1 << 14)->Complexity();
BENCHMARK_MAIN();


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license.  See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>

#include <dpMM/distribution.hpp>
#include <dpMM/normalSphere.hpp>

template<typename T>
class MF : public Distribution<T>
{
public:

//  MF(const Matrix<T,3,3>& R, const NormalSphere<T>& TGs,
//      boost::mt19937 *pRndGen);
//  MF();
  MF(const Matrix<T,3,3>& R,
      const Cat<T>& pi,
      const std::vector<NormalSphere<T> >& TGs);
//      boost::mt19937 *pRndGen);
  MF(const MF<T>& mf);
  ~MF();

  T logPdf(const Matrix<T,Dynamic,1>& x) const;

  const Matrix<T,3,3>& R() const {return R_;};
  const Cat<T>& Pi() const {return pi_;};
  const Matrix<T,Dynamic,Dynamic>& Sigma(uint32_t j) const
  {return TGs_[j].Sigma();};
  const std::vector<NormalSphere<T> >& TGs() const
  {return TGs_;};

  void print() const;

//  boost::mt19937 *pRndGen_;
protected:
  Matrix<T,3,3> R_;
  std::vector<NormalSphere<T> > TGs_;
  Cat<T> pi_;
};

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

//template<typename T>
//MF<T>::MF(const Matrix<T,3,3>& R, const NormalSphere<T>& TG,
//      boost::mt19937 *pRndGen)
//  : R_(R), TGs_(6,TG), pRndGen_(pRndGen)
//{};
template<typename T>
MF<T>::MF(const Matrix<T,3,3>& R,
    const Cat<T>& pi,
    const std::vector<NormalSphere<T> >& TGs)
//    boost::mt19937 *pRndGen)
  : Distribution<T>(NULL),
    R_(R), TGs_(TGs), pi_(pi)
//  pRndGen_(pRndGen)
{};

//template<typename T>
//MF<T>::MF()
//  : Distribution<T>(NULL),
//    TGs_(TGs), pi_(pi)
//{
//  R_ = Matrix<T,3,3>::Identity();
//  for(uint32_t k=0; k<6; ++k)
//    TGs_.push_back(NormalSphere<T>());
//
//};

template<typename T>
MF<T>::MF(const MF<T>& mf)
  : Distribution<T>(NULL),
    R_(mf.R()), TGs_(mf.TGs()), pi_(mf.Pi()) //, pRndGen_(mf.pRndGen_)
{};

template<typename T>
MF<T>::~MF()
{};

template<typename T>
T MF<T>::logPdf(const Matrix<T,Dynamic,1>& x) const
{
  Matrix<T,Dynamic,1> logPdf(6);
  logPdf.fill(0.);
  for(uint32_t k=0; k<6; ++k)
  {
    logPdf(k)= pi_.logPdf(k) + TGs_[k].logPdf(x);
//    cout<< (pi_.logPdf(k) + TGs_[k].logPdf(x)) << "\t";
  }
//  cout<<" -> "<<logPdf<<endl;
  return logSumExp<T>(logPdf);
};

template<typename T>
void MF<T>::print() const
{
  cout<<" -- MF: R, pi"<<endl
    << R_<<endl
    << pi_.pdf().transpose()<<endl;
  for(uint32_t k=0; k<6; ++k)
  {
    cout<<"TG "<<k<<": "<< TGs_[k].getMean().transpose()<<endl
      << TGs_[k].Sigma()<<endl;
  }
};
//template<typename T>
//MF<T>::()
//{};
//template<typename T>
//MF<T>::()
//{};
//template<typename T>
//MF<T>::()
//{};
//template<typename T>
//MF<T>::()
//{};
//template<typename T>
//MF<T>::()
//{};


#include <math.h>
#include <uWS/uWS.h>

#include <Eigen/Dense>
#include <boost/format.hpp>
#include <boost/log/trivial.hpp>
#include <iostream>
#include <string>

#include "json.hpp"
#include "particle_filter.h"

// for convenience
using nlohmann::json;
using std::string;
using std::vector;

// Checks if the SocketIO event has JSON data.
// If there is data the JSON object in string format will be returned,
// else the empty string "" will be returned.
namespace {
string hasData(string s) {
  auto found_null = s.find("null");
  auto b1 = s.find_first_of("[");
  auto b2 = s.find_first_of("]");
  if (found_null != string::npos) {
    return "";
  } else if (b1 != string::npos && b2 != string::npos) {
    return s.substr(b1, b2 - b1 + 1);
  }
  return "";
}
}  // namespace

int main() {
  uWS::Hub h;

  // Set up parameters here
  double delta_t = 0.1;      // Time elapsed between measurements [sec]
  double sensor_range = 50;  // Sensor range [m]

  // GPS measurement uncertainty [x [m], y [m], theta [rad]]
  const Eigen::Vector3d sigma_pose = {0.3, 0.3, 0.01};

  // Landmark measurement uncertainty [x [m], y [m]]
  const Eigen::Vector2d sigma_landmark = {0.3, 0.3};

  // Read map data
  Map map;
  if (!read_map_data("../data/map_data.txt", map)) {
    std::cout << "Error: Could not open map file" << std::endl;
    return -1;
  }

  // Create particle filter
#ifdef _OPENMP
  int num_particles = 500;
  BOOST_LOG_TRIVIAL(info) << "Found OpenMP, set num_particles to 500";
#else
  int num_particles = 100;
  BOOST_LOG_TRIVIAL(info) << "Set num_particles to 100";
#endif
  ParticleFilter pf(num_particles);

  h.onMessage([&pf, &map, &delta_t, &sensor_range, &sigma_pose,
               &sigma_landmark](uWS::WebSocket<uWS::SERVER> ws, char *data,
                                size_t length, uWS::OpCode opCode) {
    // "42" at the start of the message means there's a websocket message event.
    // The 4 signifies a websocket message
    // The 2 signifies a websocket event
    if (length && length > 2 && data[0] == '4' && data[1] == '2') {
      auto s = hasData(string(data));

      if (s != "") {
        auto j = json::parse(s);

        string event = j[0].get<string>();

        if (event == "telemetry") {
          /**
           * Read sensor measurements
           */
          vector<LandmarkObs> noisy_observations;

          // receive noisy observation data from the simulator
          // sense_observations in JSON format
          //   [{obs_x,obs_y},{obs_x,obs_y},...{obs_x,obs_y}]
          string sense_observations_x = j[1]["sense_observations_x"];
          string sense_observations_y = j[1]["sense_observations_y"];

          vector<float> x_sense;
          std::istringstream iss_x(sense_observations_x);

          std::copy(std::istream_iterator<float>(iss_x),
                    std::istream_iterator<float>(),
                    std::back_inserter(x_sense));

          vector<float> y_sense;
          std::istringstream iss_y(sense_observations_y);

          std::copy(std::istream_iterator<float>(iss_y),
                    std::istream_iterator<float>(),
                    std::back_inserter(y_sense));

          for (size_t i = 0; i < x_sense.size(); ++i) {
            LandmarkObs obs;
            obs.x = x_sense[i];
            obs.y = y_sense[i];
            noisy_observations.push_back(obs);
          }

          /**
           * Particle filter initialization and motion predictions
           *
           * This step will update the states of all particles.
           */

          // j[1] is the data JSON object
          if (!pf.initialized()) {
            // NOTE: Simulator generates GPS data.
            // Sense noisy position data from the simulator
            double sense_x = std::stod(j[1]["sense_x"].get<string>());
            double sense_y = std::stod(j[1]["sense_y"].get<string>());
            double sense_theta = std::stod(j[1]["sense_theta"].get<string>());

            pf.init(sense_x, sense_y, sense_theta, sigma_pose);
          } else {
            // Predict the vehicle's next state from previous
            //   (noiseless control) data.
            double previous_velocity =
                std::stod(j[1]["previous_velocity"].get<string>());
            double previous_yawrate =
                std::stod(j[1]["previous_yawrate"].get<string>());

            pf.prediction(delta_t, sigma_pose, previous_velocity,
                          previous_yawrate);
          }

          // Update the weights and resample
          // BOOST_LOG_TRIVIAL(info)
          //     << "Received new sensor measurements, will update particles.";

          /**
           * Update posterior probalities of all particles and update their
           * weights
           */
          pf.updateWeights(sensor_range, sigma_landmark, noisy_observations,
                           map);

          /**
           * Resample the particles using the new weights
           */
          pf.resample();

          /**
           * Select the best particle with maximum weights
           */

          // Calculate and output the average weighted error of the particle
          //   filter over all time steps so far.
          vector<Particle> particles = pf.particles;
          int num_particles = particles.size();
          double highest_weight = -std::numeric_limits<double>::infinity();
          double weight_sum = 0.0;
          int bestIndex = -1;
          for (int i = 0; i < num_particles; ++i) {
            if (particles[i].weight > highest_weight) {
              highest_weight = particles[i].weight;
              bestIndex = i;
            }
            weight_sum += particles[i].weight;
          }

          const Particle &best_particle = particles[bestIndex];

          BOOST_LOG_TRIVIAL(info)
              << (boost::format(
                      "bestIndex: %d, highest w: %.3f, average w: %.3f") %
                  bestIndex % highest_weight % (weight_sum / num_particles))
                     .str();

          /**
           * Send result message to remote simulator
           */
          json msgJson;
          msgJson["best_particle_x"] = best_particle.x;
          msgJson["best_particle_y"] = best_particle.y;
          msgJson["best_particle_theta"] = best_particle.theta;

          // Optional message data used for debugging particle's sensing
          //   and associations
          msgJson["best_particle_associations"] =
              pf.getAssociations(best_particle);
          msgJson["best_particle_sense_x"] =
              pf.getSenseCoord(best_particle, "X");
          msgJson["best_particle_sense_y"] =
              pf.getSenseCoord(best_particle, "Y");

          auto msg = "42[\"best_particle\"," + msgJson.dump() + "]";
          // BOOST_LOG_TRIVIAL(debug) << msg;
          ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
        }  // end "telemetry" if
      } else {
        string msg = "42[\"manual\",{}]";
        ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
      }
    }  // end websocket message if
  });  // end h.onMessage

  h.onConnection([&h](uWS::WebSocket<uWS::SERVER> ws, uWS::HttpRequest req) {
    BOOST_LOG_TRIVIAL(info)
        << "Connected to simulator. Waiting for start command.";
  });

  h.onDisconnection([&h](uWS::WebSocket<uWS::SERVER> ws, int code,
                         char *message, size_t length) {
    ws.close();
    BOOST_LOG_TRIVIAL(info) << "Disconnected from simulator.";
  });

  int port = 4567;
  if (h.listen(port)) {
    BOOST_LOG_TRIVIAL(info)
        << "Particle filter server started, listening to port: " << port;
  } else {
    std::cerr << "Failed to listen to port" << std::endl;
    return -1;
  }

  h.run();
}


#pragma once

#include "modprop/compo/Interfaces.h"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/bernoulli_distribution.hpp>
#include <boost/random/random_device.hpp>

namespace percepto
{

class DropoutLayer :
public Source<VectorType>
{
public:

	enum DropoutMode
	{
		DROPOUT_ENABLE,
		DROPOUT_DISABLE
	};

	typedef Source<VectorType> SourceType;
	typedef VectorType InputType;
	typedef VectorType OutputType;

	DropoutLayer()
	: _inputPort( this ),
	  _mode( DROPOUT_DISABLE )
	{}

	DropoutLayer( double p )
	: _inputPort( this ), _distribution( p ),
	  _mode( DROPOUT_DISABLE ), _p( p )
	{}

	DropoutLayer( const DropoutLayer& other )
	: _inputPort( this ), _distribution( other._distribution ),
	  _p( other._P )
	{
		SetDropoutMode( other._mode );
	}

	void SetDropoutMode( void DropoutMode mode )
	{
		_mode = mode;
		if( _mode == DROPOUT_ENABLE )
		{
			boost::random_device rng;
			_distribution.seed( rng );
		}
	}

	void Resample()
	{
		if( _mode == DROPOUT_DISABLE )
		{
			_mask.SetConstant( _p );
		}
		else
		{
			for( unsigned int i = 0; i < _mask.size(); ++i )
			{
				_mask(i) = _distribution( _generator ) ? 1.0 : 0.0;
			}
		}
	}

	virtual void Foreprop()
	{
		VectorType input = _inputPort.GetInput();

		if( _mask.size() == 0 )
		{
			_mask = VectorType::Ones( input.size() );
		}

		VectorType output = ( input.array() * _mask.array() ).matrix();
		SourceType::SetOutput( output );
		SourceType::Foreprop();
	}

	virtual void BackpropImplementation( const MatrixType& nextDodx )
	{
		MatrixType dodx = nextDodx;
		for( unsigned int i = 0; i < dodx.rows(); ++i )
		{
			VectorType row = dodx.row(i);
			dodx.row(i) = ( row.array() * _mask.array() ).
		}
	}

private:

	Sink<VectorType> _inputPort;
	VectorType _mask;
	DropoutMode _mode;
	double _p;

	boost::mt19937 _generator;
	boost::bernoulli_distribution<double> _distribution;

};

}


/**
* \file NormalInitializer.hpp
*
* \brief Glorot normal initializer, as described in http://jmlr.org/proceedings/papers/v9/glorot10a/glorot10a.pdf
*
* \date   Jun 20, 2018
* \author Mathias Bøgh Stokholm
*/

#ifndef NEURAL_NORMALINITIALIZER_HPP
#define NEURAL_NORMALINITIALIZER_HPP

#include <Eigen/Core>
#include <random>

namespace neural {
    /**
     * @brief Glorot normal initializer (also called Xavier normal initializer)
     * Draws samples from a truncated normal distribution centered on 0 with the std deviation set according to the
     * number of inputs and outputs.
     * @tparam Dtype: The data type to generate samples as
     * @tparam FanIn: The number of input units of the tensor to generate weights for
     * @tparam FanOut: The number of output units of the tensor to generate weights for
     */
    template <typename Dtype, unsigned int FanIn, unsigned int FanOut>
    class GlorotNormal {
    public:
        GlorotNormal(): m_distribution(0.0, std::sqrt(2.0 / (FanIn + FanOut))) {}
        GlorotNormal(const GlorotNormal&) = default;

        Dtype operator()(Eigen::DenseIndex element_location, Eigen::DenseIndex /*unused*/ = 0) const {
            // FIXME: This is massively hacky and ugly
            auto* nonConstThis = const_cast<GlorotNormal*>(this);
            return Dtype(nonConstThis->m_distribution(nonConstThis->m_generator));
        }

    private:
        std::mt19937 m_generator;
        std::normal_distribution<double> m_distribution;
    };
}

#endif //NEURAL_NORMALINITIALIZER_HPP


/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2006 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program is free software; you can redistribute it and/or modify       |
|  it under the terms of the GNU General Public License as published by       |
|  the Free Software Foundation; either version 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  This program 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 General Public License for more details.                               |
|                                                                             |
|  You should have received a copy of the GNU General Public License along    |
|  with this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#if ! defined(DIRICHLET_MOVE_HPP)
#define DIRICHLET_MOVE_HPP

#include <vector>									// for std::vector
#include <boost/shared_ptr.hpp>						// for boost::shared_ptr
#include <boost/weak_ptr.hpp>						// for boost::weak_ptr
#include "mcmc_updater.hpp"		// for base class MCMCUpdater
#include "partition_model.hpp"		// for PartitionModelShPtr definition
#include "multivariate_probability_distribution.hpp"

namespace phycas
{

class MCMCChainManager;
typedef boost::weak_ptr<MCMCChainManager>			ChainManagerWkPtr;

typedef boost::shared_ptr<DirichletDistribution>    DirichletShPtr;

/*----------------------------------------------------------------------------------------------------------------------
|	A DirichletMove proposes new parameter values that are slightly different than the current parameter values by
|   sampling from a Dirichlet distribution with parameters equal to the current frequencies multiplied by a large
|   value (the tuning parameter 'psi').
*/
class DirichletMove : public MCMCUpdater
	{
	public:
									DirichletMove();
									virtual ~DirichletMove()
										{
										//std::cerr << "DirichletMove dying..." << std::endl;
										}

		// Accessors
		double						getTuningParameter() const;
		unsigned					getDimension() const;

		// Modifiers
		void						setTuningParameter(double x);
		void						setMaxPsi(double x);
		void						setMinPsi(double x);
		void						setDimension(unsigned d);

		// Utilities
		void						reset();
		virtual void				sendCurrValuesToModel(const double_vect_t & v) {}
		virtual void				getCurrValuesFromModel(double_vect_t & v) const {}
		virtual double_vect_t		listCurrValuesFromModel();
        virtual void                getParams() {}
        virtual void                setParams(const std::vector<double> & v) {}

		// These are virtual functions in the MCMCUpdater base class
		virtual bool				update();
		virtual double				getLnHastingsRatio() const;
		virtual double				getLnJacobian() const;
		virtual void				proposeNewState();
		virtual void				revert();
		virtual void				accept();

	private:

		DirichletMove &				operator=(const DirichletMove &);	// never use - don't define

	protected:

		unsigned					dim;			/**< The number of parameters governed by this move */
		double						psi;			/**< Larger values result in changes of smaller magnitude */
		std::vector<double> 		new_params;	    /**< Proposed new parameter values */
		std::vector<double> 		orig_params;	/**< Saved parameter values (in case revert is necessary) */
		std::vector<double> 		c_forward;	    /**< Dirichlet parameter vector used to propose new frequencies */
		std::vector<double> 		c_reverse;	    /**< Dirichlet parameter vector used to propose original frequencies (only used to compute Hastings ratio) */
        DirichletShPtr              dir_forward;    /**< Points to an ad hoc Dirichlet distribution object used to assess the forward move density and to propose a new frequency vector */
        DirichletShPtr              dir_reverse;    /**< Points to an ad hoc Dirichlet distribution object used to assess the reverse move density */
	};

} // namespace phycas

#endif


/*
    Copyright 2005-2007 Adobe Systems Incorporated

    Use, modification and distribution are subject to the Boost Software License,
    Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
    http://www.boost.org/LICENSE_1_0.txt).

    See http://opensource.adobe.com/gil for most recent version including documentation.
*/

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

///////////////////////
////  NOTE: This sample file uses the numeric extension, which does not come with the Boost distribution.
////  You may download it from http://opensource.adobe.com/gil
///////////////////////

/// \file
/// \brief Test file for resample_pixels() in the numeric extension
/// \author Lubomir Bourdev and Hailin Jin
/// \date February 27, 2007

#include <boost/gil/image.hpp>
#include <boost/gil/typedefs.hpp>
#include <boost/gil/extension/io/jpeg_io.hpp>
#include <boost/gil/extension/numeric/sampler.hpp>
#include <boost/gil/extension/numeric/resample.hpp>

int main() {
    using namespace boost::gil;

    rgb8_image_t img;
    jpeg_read_image("test.jpg",img);

    // test resample_pixels
    // Transform the image by an arbitrary affine transformation using nearest-neighbor resampling
    rgb8_image_t transf(rgb8_image_t::point_t(view(img).dimensions()*2));
    fill_pixels(view(transf),rgb8_pixel_t(255,0,0));    // the background is red

    matrix3x2<double> mat = matrix3x2<double>::get_translate(-point2<double>(200,250)) *
                            matrix3x2<double>::get_rotate(-15*3.14/180.0);
    resample_pixels(const_view(img), view(transf), mat, nearest_neighbor_sampler());
    jpeg_write_view("out-affine.jpg", view(transf));

    return 0;
}


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

#include "DetectionPostProcess.hpp"

#include <armnn/utility/Assert.hpp>

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

#include <algorithm>
#include <numeric>

namespace armnn
{

std::vector<unsigned int> GenerateRangeK(unsigned int k)
{
    std::vector<unsigned int> range(k);
    std::iota(range.begin(), range.end(), 0);
    return range;
}

void TopKSort(unsigned int k, unsigned int* indices, const float* values, unsigned int numElement)
{
    std::partial_sort(indices, indices + k, indices + numElement,
                      [&values](unsigned int i, unsigned int j) { return values[i] > values[j]; });
}

float IntersectionOverUnion(const float* boxI, const float* boxJ)
{
    // Box-corner format: ymin, xmin, ymax, xmax.
    const int yMin = 0;
    const int xMin = 1;
    const int yMax = 2;
    const int xMax = 3;
    float areaI = (boxI[yMax] - boxI[yMin]) * (boxI[xMax] - boxI[xMin]);
    float areaJ = (boxJ[yMax] - boxJ[yMin]) * (boxJ[xMax] - boxJ[xMin]);
    float yMinIntersection = std::max(boxI[yMin], boxJ[yMin]);
    float xMinIntersection = std::max(boxI[xMin], boxJ[xMin]);
    float yMaxIntersection = std::min(boxI[yMax], boxJ[yMax]);
    float xMaxIntersection = std::min(boxI[xMax], boxJ[xMax]);
    float areaIntersection = std::max(yMaxIntersection - yMinIntersection, 0.0f) *
                                std::max(xMaxIntersection - xMinIntersection, 0.0f);
    float areaUnion = areaI + areaJ - areaIntersection;
    return areaIntersection / areaUnion;
}

std::vector<unsigned int> NonMaxSuppression(unsigned int numBoxes,
                                            const std::vector<float>& boxCorners,
                                            const std::vector<float>& scores,
                                            float nmsScoreThreshold,
                                            unsigned int maxDetection,
                                            float nmsIouThreshold)
{
    // Select boxes that have scores above a given threshold.
    std::vector<float> scoresAboveThreshold;
    std::vector<unsigned int> indicesAboveThreshold;
    for (unsigned int i = 0; i < numBoxes; ++i)
    {
        if (scores[i] >= nmsScoreThreshold)
        {
            scoresAboveThreshold.push_back(scores[i]);
            indicesAboveThreshold.push_back(i);
        }
    }

    // Sort the indices based on scores.
    unsigned int numAboveThreshold = boost::numeric_cast<unsigned int>(scoresAboveThreshold.size());
    std::vector<unsigned int> sortedIndices = GenerateRangeK(numAboveThreshold);
    TopKSort(numAboveThreshold, sortedIndices.data(), scoresAboveThreshold.data(), numAboveThreshold);

    // Number of output cannot be more than max detections specified in the option.
    unsigned int numOutput = std::min(maxDetection, numAboveThreshold);
    std::vector<unsigned int> outputIndices;
    std::vector<bool> visited(numAboveThreshold, false);

    // Prune out the boxes with high intersection over union by keeping the box with higher score.
    for (unsigned int i = 0; i < numAboveThreshold; ++i)
    {
        if (outputIndices.size() >= numOutput)
        {
            break;
        }
        if (!visited[sortedIndices[i]])
        {
            outputIndices.push_back(indicesAboveThreshold[sortedIndices[i]]);
        }
        for (unsigned int j = i + 1; j < numAboveThreshold; ++j)
        {
            unsigned int iIndex = indicesAboveThreshold[sortedIndices[i]] * 4;
            unsigned int jIndex = indicesAboveThreshold[sortedIndices[j]] * 4;
            if (IntersectionOverUnion(&boxCorners[iIndex], &boxCorners[jIndex]) > nmsIouThreshold)
            {
                visited[sortedIndices[j]] = true;
            }
        }
    }
    return outputIndices;
}

void AllocateOutputData(unsigned int numOutput,
                        unsigned int numSelected,
                        const std::vector<float>& boxCorners,
                        const std::vector<unsigned int>& outputIndices,
                        const std::vector<unsigned int>& selectedBoxes,
                        const std::vector<unsigned int>& selectedClasses,
                        const std::vector<float>& selectedScores,
                        float* detectionBoxes,
                        float* detectionScores,
                        float* detectionClasses,
                        float* numDetections)
{
    for (unsigned int i = 0; i < numOutput; ++i)
        {
            unsigned int boxIndex = i * 4;
            if (i < numSelected)
            {
                unsigned int boxCornorIndex = selectedBoxes[outputIndices[i]] * 4;
                detectionScores[i] = selectedScores[outputIndices[i]];
                detectionClasses[i] = boost::numeric_cast<float>(selectedClasses[outputIndices[i]]);
                detectionBoxes[boxIndex] = boxCorners[boxCornorIndex];
                detectionBoxes[boxIndex + 1] = boxCorners[boxCornorIndex + 1];
                detectionBoxes[boxIndex + 2] = boxCorners[boxCornorIndex + 2];
                detectionBoxes[boxIndex + 3] = boxCorners[boxCornorIndex + 3];
            }
            else
            {
                detectionScores[i] = 0.0f;
                detectionClasses[i] = 0.0f;
                detectionBoxes[boxIndex] = 0.0f;
                detectionBoxes[boxIndex + 1] = 0.0f;
                detectionBoxes[boxIndex + 2] = 0.0f;
                detectionBoxes[boxIndex + 3] = 0.0f;
            }
        }
        numDetections[0] = boost::numeric_cast<float>(numSelected);
}

void DetectionPostProcess(const TensorInfo& boxEncodingsInfo,
                          const TensorInfo& scoresInfo,
                          const TensorInfo& anchorsInfo,
                          const TensorInfo& detectionBoxesInfo,
                          const TensorInfo& detectionClassesInfo,
                          const TensorInfo& detectionScoresInfo,
                          const TensorInfo& numDetectionsInfo,
                          const DetectionPostProcessDescriptor& desc,
                          Decoder<float>& boxEncodings,
                          Decoder<float>& scores,
                          Decoder<float>& anchors,
                          float* detectionBoxes,
                          float* detectionClasses,
                          float* detectionScores,
                          float* numDetections)
{
    IgnoreUnused(anchorsInfo, detectionClassesInfo, detectionScoresInfo, numDetectionsInfo);

    // Transform center-size format which is (ycenter, xcenter, height, width) to box-corner format,
    // which represents the lower left corner and the upper right corner (ymin, xmin, ymax, xmax)
    std::vector<float> boxCorners(boxEncodingsInfo.GetNumElements());

    const unsigned int numBoxes  = boxEncodingsInfo.GetShape()[1];
    const unsigned int numScores = scoresInfo.GetNumElements();

    for (unsigned int i = 0; i < numBoxes; ++i)
    {
        // Y
        float boxEncodingY = boxEncodings.Get();
        float anchorY      = anchors.Get();

        ++boxEncodings;
        ++anchors;

        // X
        float boxEncodingX = boxEncodings.Get();
        float anchorX      = anchors.Get();

        ++boxEncodings;
        ++anchors;

        // H
        float boxEncodingH = boxEncodings.Get();
        float anchorH      = anchors.Get();

        ++boxEncodings;
        ++anchors;

        // W
        float boxEncodingW = boxEncodings.Get();
        float anchorW      = anchors.Get();

        ++boxEncodings;
        ++anchors;

        float yCentre = boxEncodingY / desc.m_ScaleY * anchorH + anchorY;
        float xCentre = boxEncodingX / desc.m_ScaleX * anchorW + anchorX;

        float halfH = 0.5f * expf(boxEncodingH / desc.m_ScaleH) * anchorH;
        float halfW = 0.5f * expf(boxEncodingW / desc.m_ScaleW) * anchorW;

        unsigned int indexY = i * 4;
        unsigned int indexX = indexY + 1;
        unsigned int indexH = indexX + 1;
        unsigned int indexW = indexH + 1;

        // ymin
        boxCorners[indexY] = yCentre - halfH;
        // xmin
        boxCorners[indexX] = xCentre - halfW;
        // ymax
        boxCorners[indexH] = yCentre + halfH;
        // xmax
        boxCorners[indexW] = xCentre + halfW;

        ARMNN_ASSERT(boxCorners[indexY] < boxCorners[indexH]);
        ARMNN_ASSERT(boxCorners[indexX] < boxCorners[indexW]);
    }

    unsigned int numClassesWithBg = desc.m_NumClasses + 1;

    // Decode scores
    std::vector<float> decodedScores;
    decodedScores.reserve(numScores);

    for (unsigned int i = 0u; i < numScores; ++i)
    {
        decodedScores.emplace_back(scores.Get());
        ++scores;
    }

    // Perform Non Max Suppression.
    if (desc.m_UseRegularNms)
    {
        // Perform Regular NMS.
        // For each class, perform NMS and select max detection numbers of the highest score across all classes.
        std::vector<float> classScores(numBoxes);

        std::vector<unsigned int> selectedBoxesAfterNms;
        selectedBoxesAfterNms.reserve(numBoxes);

        std::vector<float> selectedScoresAfterNms;
        selectedBoxesAfterNms.reserve(numScores);

        std::vector<unsigned int> selectedClasses;

        for (unsigned int c = 0; c < desc.m_NumClasses; ++c)
        {
            // For each boxes, get scores of the boxes for the class c.
            for (unsigned int i = 0; i < numBoxes; ++i)
            {
                classScores[i] = decodedScores[i * numClassesWithBg + c + 1];
            }
            std::vector<unsigned int> selectedIndices = NonMaxSuppression(numBoxes,
                                                                          boxCorners,
                                                                          classScores,
                                                                          desc.m_NmsScoreThreshold,
                                                                          desc.m_DetectionsPerClass,
                                                                          desc.m_NmsIouThreshold);

            for (unsigned int i = 0; i < selectedIndices.size(); ++i)
            {
                selectedBoxesAfterNms.push_back(selectedIndices[i]);
                selectedScoresAfterNms.push_back(classScores[selectedIndices[i]]);
                selectedClasses.push_back(c);
            }
        }

        // Select max detection numbers of the highest score across all classes
        unsigned int numSelected = boost::numeric_cast<unsigned int>(selectedBoxesAfterNms.size());
        unsigned int numOutput = std::min(desc.m_MaxDetections,  numSelected);

        // Sort the max scores among the selected indices.
        std::vector<unsigned int> outputIndices = GenerateRangeK(numSelected);
        TopKSort(numOutput, outputIndices.data(), selectedScoresAfterNms.data(), numSelected);

        AllocateOutputData(detectionBoxesInfo.GetShape()[1], numOutput, boxCorners, outputIndices,
                           selectedBoxesAfterNms, selectedClasses, selectedScoresAfterNms,
                           detectionBoxes, detectionScores, detectionClasses, numDetections);
    }
    else
    {
        // Perform Fast NMS.
        // Select max scores of boxes and perform NMS on max scores,
        // select max detection numbers of the highest score
        unsigned int numClassesPerBox = std::min(desc.m_MaxClassesPerDetection, desc.m_NumClasses);
        std::vector<float> maxScores;
        std::vector<unsigned int>boxIndices;
        std::vector<unsigned int>maxScoreClasses;

        for (unsigned int box = 0; box < numBoxes; ++box)
        {
            unsigned int scoreIndex = box * numClassesWithBg + 1;

            // Get the max scores of the box.
            std::vector<unsigned int> maxScoreIndices = GenerateRangeK(desc.m_NumClasses);
            TopKSort(numClassesPerBox, maxScoreIndices.data(),
                decodedScores.data() + scoreIndex, desc.m_NumClasses);

            for (unsigned int i = 0; i < numClassesPerBox; ++i)
            {
                maxScores.push_back(decodedScores[scoreIndex + maxScoreIndices[i]]);
                maxScoreClasses.push_back(maxScoreIndices[i]);
                boxIndices.push_back(box);
            }
        }

        // Perform NMS on max scores
        std::vector<unsigned int> selectedIndices = NonMaxSuppression(numBoxes, boxCorners, maxScores,
                                                                      desc.m_NmsScoreThreshold,
                                                                      desc.m_MaxDetections,
                                                                      desc.m_NmsIouThreshold);

        unsigned int numSelected = boost::numeric_cast<unsigned int>(selectedIndices.size());
        unsigned int numOutput = std::min(desc.m_MaxDetections,  numSelected);

        AllocateOutputData(detectionBoxesInfo.GetShape()[1], numOutput, boxCorners, selectedIndices,
                           boxIndices, maxScoreClasses, maxScores,
                           detectionBoxes, detectionScores, detectionClasses, numDetections);
    }
}

} // namespace armnn


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

#include <core/board/group.hpp>         // axioms::is_realized, make
#include <dctl/core/board/angle.hpp>    // _deg, inverse, rotate
#include <boost/test/unit_test.hpp>     // BOOST_AUTO_TEST_SUITE, BOOST_AUTO_TEST_SUITE_END, BOOST_AUTO_TEST_CASE, BOOST_CHECK
#include <algorithm>                    // all_of
#include <type_traits>                  // common_type
#include <vector>                       // vector

using namespace dctl::core;
using namespace literals;

BOOST_AUTO_TEST_SUITE(GroupCyclic)

BOOST_AUTO_TEST_CASE(GroupAxiomsAreRealizedOnCyclicGroups)
{
        constexpr auto op = [](auto i, auto j) { return rotate(i, j); };
        constexpr auto inv = [](auto i) { return inverse(i); };

        auto const C1 = make_group(
                { 0_deg },
                op, inv
        );

        auto const C2 = make_group(
                { 0_deg, 180_deg },
                op, inv
        );

        auto const C4 = make_group(
                { 0_deg,  90_deg, 180_deg, 270_deg },
                op, inv
        );

        auto const C8 = make_group(
                {   0_deg,  45_deg,  90_deg, 135_deg,
                  180_deg, 225_deg, 270_deg, 315_deg },
                op, inv
        );

        using CyclicGroup = std::common_type_t<decltype(C1), decltype(C2), decltype(C4), decltype(C8)>;

        auto const C_N = std::vector<CyclicGroup>
        {
                C1, C2, C4, C8
        };

        BOOST_CHECK(
                std::all_of(C_N.begin(), C_N.end(), [](auto const& g) {
                        return group::axioms::is_realized(g);
                })
        );
}

BOOST_AUTO_TEST_SUITE_END()


#include "shape.hpp"
#include <cmath>
#include <boost/math/constants/constants.hpp>

namespace bg = boost::geometry;

const double pi = boost::math::constants::pi<double>();
const int earthRadius = 6378137;

// Registers a bounding box
Shape::Shape(std::string id_, box const& b) {
  type = ShapeType::bbox;
  id = id_;
  envelope = b;
}

// Registers a circle
Shape::Shape(std::string id_, point const& p, double radius) {
  type = ShapeType::circle;
  id = id_;
  envelope = envelopeFromCircle(p, radius);
  center = p;
  outerRadius = radius;
}

// Registers an annulus
Shape::Shape(std::string id_, point const& p, double outerRadius_, double innerRadius_) {
  type = ShapeType::annulus;
  id = id_;
  envelope = envelopeFromCircle(p, outerRadius_);
  center = p;
  outerRadius = outerRadius_;
  innerRadius = innerRadius_;
}

// Registers a Polygon
Shape::Shape(std::string id_, polygon const& p) {
  type = ShapeType::poly;
  id = id_;
  envelope = bg::return_envelope<box>(p);
  polygonShape = p;
}

// Rough & fast approximation, should be enough to get a circle's MBR
box Shape::envelopeFromCircle(point const& p, double radius) {
  double
    lat = bg::get<1>(p),
    lon = bg::get<0>(p),
    dlat = radius / earthRadius,
    dlon = radius / (earthRadius*cos(pi*lat/180));

  dlat *= 180 / pi;
  dlon *= 180 / pi;

  return box(point(lon - dlon, lat - dlat), point(lon + dlon, lat + dlat));
}

bool Shape::covered(point const& p) {
  double distance;

  switch(type) {
    case ShapeType::bbox:
      /*
       the r* tree index handles bounding boxes so for boxes shapes,
       the approximation is the result
       */
      return true;
    case ShapeType::circle:
      return bg::distance(center, p) <= outerRadius;
    case ShapeType::annulus:
      distance = bg::distance(center, p);
      return distance <= outerRadius && distance >= innerRadius;
    case ShapeType::poly:
      return bg::covered_by(p, polygonShape);
  }

  return false;
}

const char *Shape::getId() {
  return id.c_str();
}

box Shape::getEnvelope() {
  return envelope;
}