#pragma once

#define GLM_ENABLE_EXPERIMENTAL 1
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Halide.h>
#include <fstream>
#include <glm/ext.hpp>
#include <glm/glm.hpp>
#include <halide_image_io.h>
#include <iostream>
#include <stdio.h>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <filesystem>
#include <ImfRgbaFile.h>
#include <ImfStringAttribute.h>
#include <ImfMatrixAttribute.h>
#include <ImfArray.h>
#include <algorithm>
#include <ImfNamespace.h>
#include <tuple>
#include <math.h>

#include "utilities.h"

namespace IMF = OPENEXR_IMF_NAMESPACE;

using namespace IMF;
using namespace IMATH_NAMESPACE;

using std::string;
using std::stringstream;
using std::vector;

using namespace std;
using namespace Halide;
using namespace Halide::Tools;
using namespace Eigen;
using std::filesystem::directory_iterator;

using Vector4h = Matrix<Halide::Expr, 4, 1>;
using Matrix4h = Matrix<Halide::Expr, 4, 4>;

Var x, y, c, i, pcx, pcy;

struct Point
{
    float x;
    float y;
    float z;
    uint16_t r;
    uint16_t g;
    uint16_t b;
};

void saveImage(Expr result, size_t width, size_t height, const string &basename)
{
    Target target = get_host_target();
    Func byteResult;
    byteResult(x, y) = cast<uint8_t>(clamp(result, 0.0f, 1.0f) * 255.0f);
    byteResult.compile_jit(target);
    Buffer<uint8_t> output(width, height);
    byteResult.realize(output);
    stringstream filename;
    filename << basename << ".png";
    save_image(output, filename.str());
}

Buffer<uint8_t> loadImages(vector<string> filenames)
{
    if (filenames.empty())
    {
        throw std::invalid_argument("List of filenames cannot be empty");
    }
    vector<Buffer<uint8_t>> images;
    Func assigner;
    assigner(x, y, c, i) = cast<uint8_t>(0);
    for (int i = 0; i < filenames.size(); i++)
    {
        const auto filename = filenames[i];
        images.push_back(load_image(filename));
        assigner(x, y, c, i) = images[i](x, y, c);
    }
    Buffer<uint8_t> input(images[0].width(), images[0].height(), images[0].channels(), images.size());
    assigner.realize(input);
    return input;
}

Vector4h makeAxBzCLine(Vector4h a, Vector4h b)
{
    Vector4h result;
    result(0) = a(2) - b(2);
    result(1) = b(0) - a(0);
    result(2) = a(0) * b(2) - b(0) * a(2);
    result(3) = 1.0f;
    return result;
}

Vector4h cross3(Vector4h a, Vector4h b)
{
    Vector4h res;
    res(0) = a(1) * b(2) - a(2) * b(1);
    res(1) = a(2) * b(0) - a(0) * b(2);
    res(2) = a(0) * b(1) - a(1) * b(0);
    res(3) = 0;
    return res;
}

Expr dot3(Vector4h a, Vector4h b)
{
    Expr res;
    res = a(0) * b(0) + a(1) * b(1) + a(2) * b(2);
    return res;
}

tuple<Vector4h, Vector4h> pluckerLine(Vector4h p1, Vector4h p2)
{
    Vector4h l;
    Vector4h l_dash;
    l = p1(3) * p2 - p2(3) * p1;
    l(3) = 0;
    l_dash = cross3(p1, p2);
    return {l, l_dash};
}

Vector4h pluckerPlane(Vector4h l, Vector4h l_dash, Vector4h pluckPt)
{
    Vector4h u;
    u = -pluckPt(3) * l_dash + cross3(pluckPt, l);
    u(3) = dot3(pluckPt, l_dash);
    return u;
}

Vector4h intersectionLinePlane(Vector4h l, Vector4h l_dash, Vector4h plPlane)
{
    Vector4h intersection;
    intersection = -plPlane(3) * l + cross3(plPlane, l_dash);
    intersection(3) = dot3(plPlane, l);
    return intersection;
}

bool conditionProjector(float x, float y, float z)
{
    return (x < -10.0f || x > 10.0f || y < -10.0f || y > 10.0f || z < -10.0f || z > 10.0f);
}

bool noCondition(float x, float y, float z)
{
    return false;
}

void saveImages(Expr result, size_t width, size_t height, size_t imageCount, const string &basename)
{
    Target target = get_host_target();
    Func byteResult;
    byteResult(x, y, i) = cast<uint8_t>(clamp(result, 0.0f, 1.0f) * 255.0f);
    byteResult.compile_jit(target);
    Buffer<uint8_t> output(width, height, imageCount);
    byteResult.realize(output);
    for (int i = 0; i < imageCount; i++)
    {
        stringstream filename;
        filename << basename << i << ".png";
        const Buffer<uint8_t> image = output.sliced(2, i);
        save_image(image, filename.str());
    }
}

template <typename Function>
void writeBufferToXYZFile(Buffer<float> &buffer, Buffer<uint8_t> &color, string filename, string deliminator, Function condFunc)
{
    std::vector<Point> points;
    for (int j = 0; j < buffer.height(); j++)
    {
        for (int i = 0; i < buffer.width(); i++)
        {
            const auto x = buffer(i, j, 0);
            const auto y = buffer(i, j, 1);
            const auto z = buffer(i, j, 2);
            uint8_t r = color(i, j, 0);
            uint8_t g = color(i, j, 1);
            uint8_t b = color(i, j, 2);
            if (condFunc(x, y, z))
            {
                continue;
            }
            points.push_back({x, y, z, r, g, b});
        }
    }
    std::ofstream outFile;
    outFile.open(filename);
    outFile << points.size() << "\n";
    const auto d = deliminator;
    for (const auto &point : points)
    {
        outFile << point.x << d << point.y << d << point.z << d << point.r << d << point.g << d << point.b << "\n";
    }
}

void debugImageEXR(Expr channel1Expr, Expr channel2Expr, Expr channel3Expr, int width, int height, const char fileName[])
{
    channel1Expr = cast<float>(channel1Expr);
    channel2Expr = cast<float>(channel2Expr);
    channel3Expr = cast<float>(channel3Expr);
    Target target = get_host_target();
    Func byteResult;
    byteResult(x, y, c) = 0.0f;
    byteResult(x, y, 0) = channel1Expr;
    byteResult(x, y, 1) = channel2Expr;
    byteResult(x, y, 2) = channel3Expr;
    Buffer<float> output(width, height, 3);
    byteResult.compile_jit(target);
    byteResult.realize(output);

    Array2D<Rgba> pixels;
    pixels.resizeErase(height, width);
    for (int row{0}; row < height; row++)
    {
        for (int col{0}; col < width; col++)
        {
            pixels[row][col].r = output(col, row, 0);
            pixels[row][col].g = output(col, row, 1);
            pixels[row][col].b = output(col, row, 2);
        }
    }
    RgbaOutputFile file(fileName, width, height, WRITE_RGBA);
    file.setFrameBuffer(&pixels[0][0], 1, width);
    file.writePixels(height);
}

int main(int argc, char **argv)
{
    Buffer<uint8_t> input = load_image("images/scan-images/scan_30.png");

    Expr redFilter = input(x, y, 0) > 140;

    Matrix4h K = read4x4MatFromCSV("matrices/cam-mat.csv");
    Matrix4h invK = read4x4MatFromCSV("matrices/inv-cam-mat.csv");
    Matrix4h T_CP = read4x4MatFromCSV("matrices/T-C1-C2.csv");
    Matrix4h T_WC = read4x4MatFromCSV("matrices/T-W-C1.csv");
    Vector4h t_CP{0, 0, 0, 0};
    t_CP(0) = T_CP(0, 3);
    t_CP(1) = T_CP(1, 3);
    t_CP(2) = T_CP(2, 3);
    t_CP(3) = T_CP(3, 3);
    cout << t_CP << endl;

    Vector4h scanNormal_P{1.0f, 0.0f, 0.0f, 1.0f};
    Vector4h scanPlane_C = T_CP * scanNormal_P;
    scanPlane_C(3) = -dot3(scanPlane_C, t_CP);

    cout << scanPlane_C << endl;

    Vector4h px{x, y, 1.0f, 0.0f};
    Vector4h normCoord_C = invK * px;
    Vector4h lineDash{0.0f, 0.0f, 0.0f, 0.0f};
    Vector4h intersect = intersectionLinePlane(normCoord_C, lineDash, scanPlane_C);
    intersect = intersect * redFilter;
    intersect = intersect / intersect[3];
    intersect = T_WC * intersect;
    intersect = intersect / intersect[3];
    //saveImage(intersect[2], 1920, 1080, "images/debug/depth.png");

    Func result;
    result(x, y, c) = 0.0f;
    result(x, y, 0) = intersect[0];
    result(x, y, 1) = intersect[1];
    result(x, y, 2) = intersect[2];

    Buffer<float> output(1920, 1080, 3);
    result.realize(output);
    vector<Point> points;
    std::ofstream outFile;
    outFile.open("pointclouds/singleScan.txt");

    for (int xxx = 0; xxx < output.width(); xxx++)
    {
        for (int yyy = 0; yyy < output.height(); yyy++)
        {

            float xPoint = output(xxx, yyy, 0);
            float yPoint = output(xxx, yyy, 1);
            float zPoint = output(xxx, yyy, 2);
            if (abs(zPoint > 0.01))
            {
                outFile << xPoint << ";" << yPoint << ";" << zPoint << "\n";
            }
            points.push_back({xPoint, yPoint, zPoint, 0, 0, 0});
        }
    }
    outFile.close();

    //debugImageEXR(intersect[2], intersect[2], intersect[2], 1920, 1080, "images/debug/depth.exr");
}


//File: incremental_bundle_adjuster.hh
//Date:
//Author: Yuxin Wu <ppwwyyxxc@gmail.com>

#pragma once

#pragma once
#include <vector>
#include <set>
#include <Eigen/Dense>

#include "lib/mat.h"
#include "lib/utils.hh"
#include "common/common.hh"

namespace pano {
class Camera;
struct MatchInfo;
struct Shape2D;

class IncrementalBundleAdjuster {
	public:
		// statistics of error
		struct ErrorStats {
			std::vector<double> residuals;
			double max, avg;
			ErrorStats(int size): residuals(size) {}

			inline int num_terms() const { return residuals.size(); }

			void update_stats(int inlier_threshold);
		};

		IncrementalBundleAdjuster(
				std::vector<Camera>& cameras);

		IncrementalBundleAdjuster(const IncrementalBundleAdjuster&) = delete;
		IncrementalBundleAdjuster& operator = (const IncrementalBundleAdjuster&) = delete;

    // m is matches[j][i] in stitcher (i.e. from i to j)
		void add_match(int i, int j, const MatchInfo& m);

		void optimize();

		ErrorStats get_error_stat() {
			ParamState state;
			for (auto& c: result_cameras) state.cameras.emplace_back(c);
			state.ensure_params();
			return calcError(state);
		}

	protected:
		std::vector<Camera>& result_cameras;

		struct MatchPair {
			int from, to;		// the original image index
			const MatchInfo& m;
			MatchPair(int i, int j, const MatchInfo& m):
				from(i), to(j), m(m){}
		};

		int nr_pointwise_match = 0;
		int inlier_threshold = std::numeric_limits<int>::max();
		std::vector<MatchPair> match_pairs;

		// original indices that have appeared so far
		std::set<int> idx_added;

		// map from original image index to index being added
		std::vector<int> index_map;
		// map from index in match_pairs to the index of its first error term
		std::vector<int> match_cnt_prefix_sum;

		inline void update_index_map() {
			int cnt = 0;
			for (auto& i : idx_added)
				index_map[i] = cnt++;
		}

		// state of all parameters estimated so far
		// could be in the form of either cameras or params
		struct ParamState {
			std::vector<Camera> cameras;
			std::vector<double> params;

			std::vector<Camera>& get_cameras();
			const std::vector<Camera>& get_cameras() const {
				return const_cast<ParamState*>(this)->get_cameras();
			}

			void ensure_params() const;

			const std::vector<double>& get_params() const
			{ ensure_params(); return params; }

			std::vector<double>& get_params()
			{ ensure_params(); return params; }

			// change a param, and its corresponding camera
			void mutate_param(int param_idx, double new_val);
		};

		/// Optimization routines:
		Eigen::MatrixXd J, JtJ;		// to avoid too many malloc

		ErrorStats calcError(const ParamState& state);

		Eigen::VectorXd get_param_update(
				const ParamState& state, const std::vector<double>& residual, float);

		// calculate J & JtJ
		void calcJacobianNumerical(const ParamState& state);
		void calcJacobianSymbolic(const ParamState& state);

};

}


/*
Copyright 2012-2014 Rogier van Dalen.

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.
*/

/** \file
Tests for arithmetic_magma.hpp for integer types.
*/

#ifndef MATH_TEST_ARITHMETIC_MAGMA_TESTS_INTEGER_HPP_INCLUDED
#define MATH_TEST_ARITHMETIC_MAGMA_TESTS_INTEGER_HPP_INCLUDED

#include "math/arithmetic_magma.hpp"

#include <boost/mpl/assert.hpp>

#include "range/std/container.hpp"

#include "math/check/check_magma.hpp"

/* Produce example values. */

/**
Get signed integer examples for test.
Note that overflow yields undefined behaviour on signed integers.
Therefore, these values make sure to stay well clear of overflows, even on
checking associativity (a * a * a).
*/
template <class Type> std::vector <Type> get_signed_integer_examples() {
    std::vector <Type> examples;

    examples.push_back (0);
    examples.push_back (1);
    examples.push_back (3);
    examples.push_back (5);

    examples.push_back (-1);
    examples.push_back (-3);
    examples.push_back (-5);

    return std::move (examples);
}

/**
Get unsigned integer examples for test.
For unsigned integers, overflow is defined, so this will yield the minimum and
maximum values possible.
*/
template <class Type> std::vector <Type> get_unsigned_integer_examples() {
    std::vector <Type> examples;

    Type min = std::numeric_limits <Type>::min();
    examples.push_back (min);
    Type max = std::numeric_limits <Type>::max();
    examples.push_back (max);
    examples.push_back (0);
    examples.push_back (-1);

    examples.push_back (1);
    examples.push_back (3);
    examples.push_back (5);
    examples.push_back (-3);
    examples.push_back (-5);

    return std::move (examples);
}

/* Actual tests. */

template <class Type, class Examples>
    void test_arithmetic_magma_integer (Examples const & examples)
{
    Type a = 3;
    Type b = 5;

    namespace callable = math::callable;
    using math::has;

    // Always available; but always true.
    BOOST_MPL_ASSERT ((has <callable::is_member (Type &)>));
    BOOST_CHECK (math::is_member (a));
    BOOST_MPL_ASSERT ((std::is_same <
        decltype (math::is_member (a)), rime::true_type>));

    BOOST_MPL_ASSERT ((has <callable::equal (Type const &, Type)>));
    BOOST_CHECK (math::equal (a, a));
    BOOST_CHECK (math::equal (b, b));
    BOOST_CHECK (!math::equal (a, b));
    BOOST_CHECK (!math::equal (b, a));

    BOOST_MPL_ASSERT ((
        has <callable::approximately_equal (Type const &, Type)>));

    // Produce.
    BOOST_MPL_ASSERT_NOT ((has <callable::non_member <Type const> ()>));

    BOOST_MPL_ASSERT ((has <callable::identity <Type, callable::plus>()>));
    BOOST_MPL_ASSERT ((has <callable::identity <Type> (callable::plus)>));
    BOOST_CHECK_EQUAL ((math::identity <Type, callable::plus>()), 0);
    BOOST_CHECK_EQUAL ((math::identity <Type> (math::plus)), 0);

    BOOST_MPL_ASSERT ((has <callable::identity <Type, callable::times>()>));
    BOOST_CHECK_EQUAL ((math::identity <Type, callable::times>()), 1);
    BOOST_CHECK_EQUAL ((math::identity <Type> (math::times)), 1);

    BOOST_MPL_ASSERT_NOT ((
        has <callable::annihilator <Type, callable::plus>()>));
    BOOST_MPL_ASSERT ((has <callable::annihilator <Type, callable::times>()>));
    BOOST_CHECK_EQUAL ((math::annihilator <Type, callable::times>()), 0);

    BOOST_CHECK_EQUAL (math::zero <Type>(), 0);
    BOOST_CHECK_EQUAL (math::one <Type>(), 1);

    // Operations.
    BOOST_MPL_ASSERT ((has <callable::compare (Type, Type)>));
    BOOST_MPL_ASSERT_NOT ((math::is::approximate <
        callable::compare (Type, Type)>));
    BOOST_CHECK_EQUAL (math::compare (a, a), false);
    BOOST_CHECK_EQUAL (math::compare (a, b), true);
    BOOST_CHECK_EQUAL (math::compare (b, a), false);

    static_assert (has <callable::pick (bool, Type, Type)>::value, "");
    static_assert (
        has <callable::pick (rime::true_type, Type, Type)>::value, "");
    static_assert (
        has <callable::pick (std::false_type, Type, Type)>::value, "");

    BOOST_CHECK_EQUAL (math::pick (true, a, b), a);
    BOOST_CHECK_EQUAL (math::pick (false, a, b), b);
    BOOST_CHECK_EQUAL (math::pick (std::true_type(), b, a), b);
    BOOST_CHECK_EQUAL (math::pick (rime::false_, b, a), a);

    BOOST_MPL_ASSERT_NOT ((has <callable::choose (Type, Type)>));

    BOOST_MPL_ASSERT ((has <callable::times (Type, Type)>));
    BOOST_MPL_ASSERT_NOT ((math::is::approximate <
        callable::times (Type, Type)>));
    BOOST_CHECK_EQUAL (math::times (a, b), 15);

    BOOST_MPL_ASSERT ((has <callable::plus (Type, Type)>));
    BOOST_MPL_ASSERT_NOT ((
        math::is::approximate <callable::plus (Type, Type)>));
    BOOST_CHECK_EQUAL (math::plus (a, b), 8);

    BOOST_MPL_ASSERT_NOT ((has <callable::divide<> (Type, Type)>));
    BOOST_MPL_ASSERT_NOT ((has <callable::divide <math::left> (Type, Type)>));
    BOOST_MPL_ASSERT_NOT ((has <callable::divide <math::right> (Type, Type)>));

    BOOST_MPL_ASSERT_NOT ((has <
        callable::invert <math::left, math::callable::times> (Type)>));
    BOOST_MPL_ASSERT_NOT ((has <
        callable::invert <math::right, math::callable::times> (Type)>));
    BOOST_MPL_ASSERT_NOT ((has <
        callable::invert <math::either, math::callable::times> (Type)>));
    BOOST_MPL_ASSERT_NOT ((has <
        callable::invert <math::callable::times> (Type)>));

    // Properties
    BOOST_MPL_ASSERT ((math::is::associative <callable::times (Type, Type &)>));
    BOOST_MPL_ASSERT ((math::is::commutative <callable::times (Type& , Type)>));
    BOOST_MPL_ASSERT_NOT ((math::is::idempotent <
        callable::times (Type, Type)>));
    BOOST_MPL_ASSERT_NOT ((math::is::path_operation <
        callable::times (Type &, Type const &)>));

    BOOST_MPL_ASSERT ((math::is::associative <callable::plus (Type, Type &)>));
    BOOST_MPL_ASSERT ((math::is::commutative <callable::plus (Type& , Type)>));
    BOOST_MPL_ASSERT_NOT ((math::is::idempotent <
        callable::plus (Type, Type)>));
    BOOST_MPL_ASSERT_NOT ((math::is::path_operation <
        callable::plus (Type &, Type const &)>));

    BOOST_MPL_ASSERT ((math::is::distributive <
        math::left, callable::times, callable::plus, Type>));

    BOOST_MPL_ASSERT ((math::is::distributive <
        callable::times (callable::plus (Type, Type const), Type &)>));
    BOOST_MPL_ASSERT ((math::is::distributive <
        callable::times (Type, callable::plus (Type &, Type const &))>));

    BOOST_MPL_ASSERT ((math::is::semiring <math::either,
        callable::times, callable::plus, Type>));

    // Check for consistency.
    math::check_semiring <Type, math::either> (
        math::times, math::plus, examples);
}

#endif // MATH_TEST_ARITHMETIC_MAGMA_TESTS_INTEGER_HPP_INCLUDED


// Date Last Altered: $Date: 2006-11-15 20:51:22 -0500 (Wed, 15 Nov 2006) $
// Revision Number: $Revision: 39 $
//--------------------------------------------*-C++-*------------------------------------//
/// \file: SweepIter.hh
/// \author Greg Davidson
/// \date February 6, 2006
///
/// \brief This file provides the sweep iterator class.
///
/// This file provides the SweepIter class.  This class provides the ability to
/// iterate over the zones in a sweeping pattern, dependent upon the streaming
/// angle.

#ifndef SWEEPITER_HH
#define SWEEPITER_HH

#include <iterator>
#include <cmath>
#include <vector>
#include <limits>

#include <boost/utility.hpp>

#include "OneDCartMesh.hh"
#include "IteratorExcept.hh"

using std::iterator;
using std::vector;
using std::fabs;
using std::numeric_limits;
using boost::addressof;

/**  \addtogroup MeshSys Mesh Subsystem
 *   @{
 */
/// \brief  The SweepIter class iterates over the zones in a sweep pattern.
///
/// The SweepIter class iterates over the zones in the correct order for a sweeping pattern
/// given the streaming angle \f$ \mu \f$.
class SweepIter : public iterator< std::bidirectional_iterator_tag, Zone >
{
public:
	/// Default constructor.
	SweepIter()
	: mMu(0), mCont(0), mIter() {  }

	/// Constructor taking an angle, a reference to the zone container, and a const_iterator.
    SweepIter(double mu, const vector<Zone>& cont_in, const vector<Zone>::const_iterator iter_in)
        : mMu(mu), mCont( addressof(cont_in) ), mIter(iter_in) {  }

    /// Copy constructor
    SweepIter(const SweepIter& iter_in)
        : mMu(iter_in.mMu), mCont( iter_in.mCont ), mIter(iter_in.mIter) {  }

    /// Retrieve the iterator into the zone vector
    const vector<Zone>::const_iterator& iterator() const { return mIter; }
    /// Retrieve the container
    const vector<Zone>& container() const { return *mCont; }
    /// Retrieve the angle
    double angle() const { return mMu; }

    /// Copy operator
    SweepIter& operator=(const SweepIter& iter_in)
    {
        mCont = addressof(iter_in.container());
        mIter = iter_in.iterator();
	mMu = iter_in.angle();
	return *this;
    }

    /// Dereference Operator
    const Zone& operator*() const throw(IteratorOutOfRange)
    {
#ifndef NITERCHK
        if( mIter == mCont->end() || mIter == mCont->begin()-1) throw(IteratorOutOfRange(__LINE__, __FILE__) );
#endif
        return *mIter;
    }
    /// Indirection operator
    const Zone* operator->() const throw(IteratorOutOfRange)
    {
#ifndef NITERCHK
        if( mIter == mCont->end() || mIter == mCont->begin()-1) throw(IteratorOutOfRange(__LINE__, __FILE__) );
#endif
        return addressof(*mIter);
    }
    /// Prefix increment operator
    SweepIter& operator++() throw(IteratorOutOfRange)
    {
#ifndef NITERCHK
        if(mMu > 0 && mIter == mCont->end() ) throw(IteratorOutOfRange(__LINE__, __FILE__) );
        else if(mMu < 0 && mIter == mCont->begin()-1) throw(IteratorOutOfRange(__LINE__, __FILE__) );
#endif
        if(mMu > 0) ++mIter;
        else --mIter;

        return *this;
    }
    /// Postfix increment operator
    SweepIter operator++(int) throw(IteratorOutOfRange)
    {
        SweepIter tmp = *this;
        ++*this;
        return tmp;
    }

    /// Prefix increment operator
    SweepIter& operator--() throw(IteratorOutOfRange)
    {
#ifndef NITERCHK
        if(mMu > 0 && mIter == mCont->begin() ) throw(IteratorOutOfRange(__LINE__, __FILE__) );
        else if(mMu < 0 && mIter == mCont->end()-1 ) throw(IteratorOutOfRange(__LINE__, __FILE__) );
#endif
        if(mMu > 0) ++mIter;
        else --mIter;

        return *this;
    }
    /// Postfix increment operator
    SweepIter operator--(int) throw(IteratorOutOfRange)
    {
        SweepIter tmp = *this;
        --*this;
        return tmp;
    }

    /// Equality operator
    bool operator==(const SweepIter& iter_in) const
    {
        if(fabs(mMu-iter_in.angle()) < numeric_limits<double>::epsilon()
		   && mCont == iter_in.mCont && mIter == iter_in.mIter) return true;
        else return false;
    }
    /// Inequality operator
    bool operator!=(const SweepIter& iter_in) const
    {
        return !operator==(iter_in);
    }

protected:
    double mMu;   ///< Angle of sweep
    const vector<Zone>* mCont;   ///< Pointer to container
    vector<Zone>::const_iterator mIter;   ///< Iterator into container
};

/// @}

#endif


#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <tuple>
#include <numeric>
#include <random>
#include <boost/program_options.hpp>
#define VEXCL_USE_CUSPARSE
#include <vexcl/devlist.hpp>
#include <vexcl/vector.hpp>
#include <vexcl/reductor.hpp>
#include <vexcl/random.hpp>
#include <vexcl/tagged_terminal.hpp>
#include <vexcl/element_index.hpp>
#include <vexcl/spmat.hpp>
#include <vexcl/stencil.hpp>
#include <vexcl/sort.hpp>
#include <vexcl/scan.hpp>

#ifdef VEXCL_HAVE_BOOST_COMPUTE
#  include <vexcl/external/boost_compute.hpp>
#endif

#ifdef VEXCL_HAVE_CLOGS
#  include <vexcl/external/clogs.hpp>
#endif

#ifdef _MSC_VER
#  pragma warning(disable : 4267)
#endif

//---------------------------------------------------------------------------
struct Options {
    bool bm_saxpy;
    bool bm_vector;
    bool bm_reductor;
    bool bm_stencil;
    bool bm_spmv;
    bool bm_rng;
    bool bm_sort;
    bool bm_scan;
    bool bm_cpu;

    Options() :
        bm_saxpy(true),
        bm_vector(true),
        bm_reductor(true),
        bm_stencil(true),
        bm_spmv(true),
        bm_rng(true),
        bm_sort(true),
        bm_scan(true),
        bm_cpu(true)
    {}

    void revert() {
        bm_saxpy    = !bm_saxpy;
        bm_vector   = !bm_vector;
        bm_reductor = !bm_reductor;
        bm_stencil  = !bm_stencil;
        bm_spmv     = !bm_spmv;
        bm_rng      = !bm_rng;
        bm_sort     = !bm_sort;
        bm_scan     = !bm_scan;
        bm_cpu      = !bm_cpu;
    }
} options;

//---------------------------------------------------------------------------
template <typename real>
std::vector<real> random_vector(size_t n) {
    std::default_random_engine rng( std::rand() );
    std::uniform_real_distribution<real> rnd(0.0, 1.0);

    std::vector<real> x(n);
    for(size_t i = 0; i < n; ++i) x[i] = rnd(rng);

    return x;
}

//---------------------------------------------------------------------------
template <typename real>
std::pair<double,double> benchmark_saxpy(
        const vex::Context &ctx, vex::profiler<> &prof
        )
{
    const size_t N = 1024 * 1024;
    const size_t M = 1024;
    double time_elapsed;

    std::vector<real> A(N, 0);
    std::vector<real> B = random_vector<real>(N);
    std::vector<real> alphavec = random_vector<real>(1);
    real alpha = alphavec[0];

    vex::vector<real> a(ctx, A);
    vex::vector<real> b(ctx, B);

    auto ta = vex::tag<1>(a);

    ta = alpha * ta + b;
    ta = static_cast<real>(0);

    prof.tic_cpu("OpenCL");
    for(size_t i = 0; i < M; i++)
        ta = alpha * ta + b;
    ctx.finish();
    time_elapsed = prof.toc("OpenCL");

    double gflops = (2.0 * N * M) / time_elapsed / 1e9;
    double bwidth = (3.0 * N * M * sizeof(real)) / time_elapsed / 1e9;

    std::cout
        << "Vector SAXPY (" << vex::type_name<real>() << ")\n"
        << "  OpenCL"
        << "\n    GFLOPS:    " << gflops
        << "\n    Bandwidth: " << bwidth
        << std::endl;

    if (options.bm_cpu) {
        prof.tic_cpu("C++");
        for(size_t i = 0; i < M; i++)
            for(size_t j = 0; j < N; j++)
                A[j] = alpha * A[j] + B[j];
        time_elapsed = prof.toc("C++");

        {
            double gflops = (2.0 * N * M) / time_elapsed / 1e9;
            double bwidth = (3.0 * N * M * sizeof(real)) / time_elapsed / 1e9;

            std::cout
                << "  C++"
                << "\n    GFLOPS:    " << gflops
                << "\n    Bandwidth: " << bwidth
                << std::endl;
        }

        vex::copy(A, b);
        vex::Reductor<real, vex::SUM> sum(ctx);

        a -= b;
        std::cout << "  res = " << sum(a * a)
                  << std::endl << std::endl;
    }

    return std::make_pair(gflops, bwidth);
}

//---------------------------------------------------------------------------
template <typename real>
std::pair<double,double> benchmark_vector(
        const vex::Context &ctx, vex::profiler<> &prof
        )
{
    const size_t N = 1024 * 1024;
    const size_t M = 1024;
    double time_elapsed;

    std::vector<real> A(N, 0);
    std::vector<real> B = random_vector<real>(N);
    std::vector<real> C = random_vector<real>(N);
    std::vector<real> D = random_vector<real>(N);

    vex::vector<real> a(ctx, A);
    vex::vector<real> b(ctx, B);
    vex::vector<real> c(ctx, C);
    vex::vector<real> d(ctx, D);

    a += b + c * d;
    a = 0;

    prof.tic_cpu("OpenCL");
    for(size_t i = 0; i < M; i++)
        a += b + c * d;
    ctx.finish();
    time_elapsed = prof.toc("OpenCL");

    double gflops = (3.0 * N * M) / time_elapsed / 1e9;
    double bwidth = (5.0 * N * M * sizeof(real)) / time_elapsed / 1e9;

    std::cout
        << "Vector arithmetic (" << vex::type_name<real>() << ")\n"
        << "  OpenCL"
        << "\n    GFLOPS:    " << gflops
        << "\n    Bandwidth: " << bwidth
        << std::endl;

    if (options.bm_cpu) {
        prof.tic_cpu("C++");
        for(size_t i = 0; i < M; i++)
            for(size_t j = 0; j < N; j++)
                A[j] += B[j] + C[j] * D[j];
        time_elapsed = prof.toc("C++");
        {
            double gflops = (3.0 * N * M) / time_elapsed / 1e9;
            double bwidth = (5.0 * N * M * sizeof(real)) / time_elapsed / 1e9;

            std::cout
                << "  C++"
                << "\n    GFLOPS:    " << gflops
                << "\n    Bandwidth: " << bwidth
                << std::endl;
        }

        vex::copy(A, b);
        vex::Reductor<real, vex::SUM> sum(ctx);

        a -= b;
        std::cout << "  res = " << sum(a * a)
                  << std::endl << std::endl;
    }

    return std::make_pair(gflops, bwidth);
}

//---------------------------------------------------------------------------
template <typename real>
std::pair<double, double> benchmark_reductor(
        const vex::Context &ctx, vex::profiler<> &prof
        )
{
    const size_t N = 16 * 1024 * 1024;
    const size_t M = 1024 / 16;
    double time_elapsed;

    std::vector<real> A = random_vector<real>(N);
    std::vector<real> B = random_vector<real>(N);

    vex::vector<real> a(ctx, A);
    vex::vector<real> b(ctx, B);

    vex::Reductor<real, vex::SUM> sum(ctx);

    real sum_cl = sum(a * b);
    sum_cl = 0;

    prof.tic_cpu("OpenCL");
    for(size_t i = 0; i < M; i++)
        sum_cl += sum(a * b);
    ctx.finish();
    time_elapsed = prof.toc("OpenCL");

    double gflops = 2.0 * N * M / time_elapsed / 1e9;
    double bwidth = 2.0 * N * M * sizeof(real) / time_elapsed / 1e9;

    std::cout
        << "Reduction (" << vex::type_name<real>() << ")\n"
        << "  OpenCL"
        << "\n    GFLOPS:    " << gflops
        << "\n    Bandwidth: " << bwidth
        << std::endl;

    if (options.bm_cpu) {
        real sum_cpp = 0;
        prof.tic_cpu("C++");
        for(size_t i = 0; i < M; i++)
            sum_cpp += std::inner_product(A.begin(), A.end(), B.begin(), static_cast<real>(0));
        time_elapsed = prof.toc("C++");

        {
            double gflops = 2.0 * N * M / time_elapsed / 1e9;
            double bwidth = 2.0 * N * M * sizeof(real) / time_elapsed / 1e9;

            std::cout
                << "  C++"
                << "\n    GFLOPS:    " << gflops
                << "\n    Bandwidth: " << bwidth
                << std::endl;
        }

        std::cout << "  res = " << fabs( (sum_cl - sum_cpp) / sum_cpp )
                  << std::endl << std::endl;
    }

    return std::make_pair(gflops, bwidth);
}

//---------------------------------------------------------------------------
template <typename real>
std::pair<double, double> benchmark_stencil(
        const vex::Context &ctx, vex::profiler<> &prof
        )
{
    const long N = 1024 * 1024;
    const long M = 1024;
    double time_elapsed;

    std::vector<real> A = random_vector<real>(N);
    std::vector<real> B(N);

    std::vector<real> S(21, static_cast<real>(1) / 21);
    long center = S.size() / 2;
    vex::stencil<real> s(ctx, S, center);

    vex::vector<real> a(ctx, A);
    vex::vector<real> b(ctx, N);

    b = a * s;

    prof.tic_cpu("OpenCL");
    for(long i = 0; i < M; i++)
        b = a * s;
    ctx.finish();
    time_elapsed = prof.toc("OpenCL");

    double gflops = 2.0 * S.size() * N * M / time_elapsed / 1e9;
    double bwidth = 2.0 * S.size() * N * M * sizeof(real) / time_elapsed / 1e9;

    std::cout
        << "Stencil convolution (" << vex::type_name<real>() << ")\n"
        << "  OpenCL"
        << "\n    GFLOPS:    " << gflops
        << "\n    Bandwidth: " << bwidth
        << std::endl;

    if (options.bm_cpu) {
        prof.tic_cpu("C++");
        for(long j = 0; j < M; j++) {
            for(long i = 0; i < N; i++) {
                real sum = 0;
                for(long k = 0; k < (long)S.size(); k++)
                    sum += S[k] * A[std::min<long>(N-1, std::max<long>(0, i + k - center))];
                B[i] = sum;
            }
        }
        time_elapsed = prof.toc("C++");

        {
            double gflops = 2.0 * S.size() * N * M / time_elapsed / 1e9;
            double bwidth = 2.0 * S.size() * N * M * sizeof(real) / time_elapsed / 1e9;

            std::cout
                << "  C++"
                << "\n    GFLOPS:    " << gflops
                << "\n    Bandwidth: " << bwidth
                << std::endl;
        }

        vex::Reductor<real, vex::MAX> max(ctx);
        copy(B, a);

        std::cout << "  res = " << max(fabs(a - b))
                  << std::endl << std::endl;
    }

    return std::make_pair(gflops, bwidth);
}

//---------------------------------------------------------------------------
template <typename real>
std::pair<double,double> benchmark_spmv(
        const vex::Context &ctx, vex::profiler<> &prof
        )
{
    // Construct matrix for 3D Poisson problem in cubic domain.
    const size_t n = 128;
    const size_t N = n * n * n;
    const size_t M = 1024;

    double time_elapsed;

    const real h2i = (n - 1) * (n - 1);

    std::vector<size_t> row;
    std::vector<uint>   col;
    std::vector<real>   val;
    std::vector<real>   X(n * n * n, static_cast<real>(1e-2));
    std::vector<real>   Y(n * n * n, 0);

    row.reserve(n * n * n + 1);
    col.reserve(6 * (n - 2) * (n - 2) * (n - 2) + n * n * n);
    val.reserve(6 * (n - 2) * (n - 2) * (n - 2) + n * n * n);

    row.push_back(0);
    for(size_t k = 0, idx = 0; k < n; k++) {
        for(size_t j = 0; j < n; j++) {
            for(size_t i = 0; i < n; i++, idx++) {
                if (
                        i == 0 || i == (n - 1) ||
                        j == 0 || j == (n - 1) ||
                        k == 0 || k == (n - 1)
                   )
                {
                    col.push_back(idx);
                    val.push_back(1);
                    row.push_back(row.back() + 1);
                } else {
                    col.push_back(idx - n * n);
                    val.push_back(-h2i);

                    col.push_back(idx - n);
                    val.push_back(-h2i);

                    col.push_back(idx - 1);
                    val.push_back(-h2i);

                    col.push_back(idx);
                    val.push_back(6 * h2i);

                    col.push_back(idx + 1);
                    val.push_back(-h2i);

                    col.push_back(idx + n);
                    val.push_back(-h2i);

                    col.push_back(idx + n * n);
                    val.push_back(-h2i);

                    row.push_back(row.back() + 7);
                }
            }
        }
    }

    size_t nnz = row.back();

    // Transfer data to compute devices.
    vex::SpMat<real,uint> A(ctx, n * n * n, n * n * n, row.data(), col.data(), val.data());

    vex::vector<real> x(ctx, X);
    vex::vector<real> y(ctx, Y);

    // Get timings.
    y += A * x;
    y = 0;

    prof.tic_cpu("OpenCL");
    for(size_t i = 0; i < M; i++)
        y += A * x;
    ctx.finish();
    time_elapsed = prof.toc("OpenCL");

    double gflops = M / time_elapsed / 1e9 * (2.0 * nnz + N);
    double bwidth = M / time_elapsed / 1e9 * (nnz * (2 * sizeof(real) + sizeof(size_t)) + 4 * N * sizeof(real));

    std::cout
        << "SpMV (" << vex::type_name<real>() << ")\n"
        << "  OpenCL"
        << "\n    GFLOPS:    " << gflops
        << "\n    Bandwidth: " << bwidth
        << std::endl;

    if (options.bm_cpu) {
        prof.tic_cpu("C++");
        for(size_t k = 0; k < M; k++)
            for(size_t i = 0; i < N; i++) {
                real s = 0;
                for(size_t j = row[i]; j < row[i + 1]; j++)
                    s += val[j] * X[col[j]];
                Y[i] += s;
            }
        time_elapsed = prof.toc("C++");

        {
            double gflops = M / time_elapsed / 1e9 * (2.0 * nnz + N);
            double bwidth = M / time_elapsed / 1e9 * (nnz * (2 * sizeof(real) + sizeof(size_t)) + 4 * N * sizeof(real));

            std::cout
                << "  C++"
                << "\n    GFLOPS:    " << gflops
                << "\n    Bandwidth: " << bwidth
                << std::endl;
        }

        copy(Y, x);

        y -= x;

        vex::Reductor<real, vex::SUM> sum(ctx);

        std::cout << "  res = " << sum(y * y) << std::endl << std::endl;
    }

    return std::make_pair(gflops, bwidth);
}

//---------------------------------------------------------------------------
template <typename real>
std::pair<double,double> benchmark_spmv_ccsr(
        const vex::Context &ctx, vex::profiler<> &prof
        )
{
    // Construct matrix for 3D Poisson problem in cubic domain.
    const uint n = 128;
    const uint N = n * n * n;
    const uint M = 1024;

    double time_elapsed;

    const real h2i = (n - 1) * (n - 1);

    std::vector<size_t> idx;
    std::vector<size_t> row(3);
    std::vector<int>    col(8);
    std::vector<real>   val(8);

    std::vector<real>   X(n * n * n, static_cast<real>(1e-2));
    std::vector<real>   Y(n * n * n, 0);

    idx.reserve(n * n * n);

    row[0] = 0;
    row[1] = 1;
    row[2] = 8;

    col[0] = 0;
    val[0] = 1;

    col[1] = -static_cast<int>(n * n);
    col[2] = -static_cast<int>(n);
    col[3] =    -1;
    col[4] =     0;
    col[5] =     1;
    col[6] =     n;
    col[7] =  (n * n);

    val[1] = -h2i;
    val[2] = -h2i;
    val[3] = -h2i;
    val[4] =  h2i * 6;
    val[5] = -h2i;
    val[6] = -h2i;
    val[7] = -h2i;

    for(size_t k = 0; k < n; k++) {
        for(size_t j = 0; j < n; j++) {
            for(size_t i = 0; i < n; i++) {
                if (
                        i == 0 || i == (n - 1) ||
                        j == 0 || j == (n - 1) ||
                        k == 0 || k == (n - 1)
                   )
                {
                    idx.push_back(0);
                } else {
                    idx.push_back(1);
                }
            }
        }
    }

    size_t nnz = 6 * (n - 2) * (n - 2) * (n - 2) + n * n * n;

    // Transfer data to compute devices.
    vex::SpMatCCSR<real,int> A(ctx.queue(0), n * n * n, 2,
            idx.data(), row.data(), col.data(), val.data());

    std::vector<vex::command_queue> q1(1, ctx.queue(0));
    vex::vector<real> x(q1, X);
    vex::vector<real> y(q1, Y);

    // Get timings.
    y += A * x;
    y = 0;

    prof.tic_cpu("OpenCL");
    for(size_t i = 0; i < M; i++)
        y += A * x;
    ctx.finish();
    time_elapsed = prof.toc("OpenCL");

    double gflops = (2.0 * nnz + N) * M / time_elapsed / 1e9;
    double bwidth = M * (nnz * (2 * sizeof(real) + sizeof(int)) + 4 * N * sizeof(real)) / time_elapsed / 1e9;

    std::cout
        << "SpMV (CCSR) (" << vex::type_name<real>() << ")\n"
        << "  OpenCL"
        << "\n    GFLOPS:    " << gflops
        << "\n    Bandwidth: " << bwidth
        << std::endl;

    if (options.bm_cpu) {
        prof.tic_cpu("C++");
        for(size_t k = 0; k < M; k++)
            for(size_t i = 0; i < N; i++) {
                real s = 0;
                for(size_t j = row[idx[i]]; j < row[idx[i] + 1]; j++)
                    s += val[j] * X[i + col[j]];
                Y[i] += s;
            }
        time_elapsed = prof.toc("C++");

        {
            double gflops = (2.0 * nnz + N) * M / time_elapsed / 1e9;
            double bwidth = M * (nnz * (2 * sizeof(real) + sizeof(int)) + 4 * N * sizeof(real)) / time_elapsed / 1e9;

            std::cout
                << "  C++"
                << "\n    GFLOPS:    " << gflops
                << "\n    Bandwidth: " << bwidth
                << std::endl;
        }

        copy(Y, x);

        y -= x;

        vex::Reductor<real, vex::SUM> sum(q1);

        std::cout << "  res = " << sum(y * y) << std::endl << std::endl;
    }

    return std::make_pair(gflops, bwidth);
}

//---------------------------------------------------------------------------
template <typename real, class GF>
double rng_throughput(const vex::Context &ctx, size_t N, size_t M) {

    vex::Random<real, GF> rnd;
    vex::Reductor<real, vex::MAX> max(ctx);

    real s = max( rnd( vex::element_index(0, N), std::rand() ) );

    vex::stopwatch<> w;

    for(size_t i = 0; i < M; i++)
        s = std::max(s, max( rnd( vex::element_index(0, N), std::rand() ) ));
    ctx.finish();

    return N * M / w.toc();
}

//---------------------------------------------------------------------------
template <typename real>
void benchmark_rng(
        const vex::Context &ctx, vex::profiler<> &prof
        )
{
    const size_t N = 16 * 1024 * 1024;
    const size_t M = 1024;

    prof.tic_cpu("OpenCL (threefry)");
    double rps = rng_throughput<real, vex::random::threefry>(ctx, N, M);
    prof.toc("OpenCL (threefry)");

    std::cout
        << "Random numbers per second (" << vex::type_name<real>() << ")\n"
        << "    OpenCL (threefry): " << rps << std::endl;

    prof.tic_cpu("OpenCL (philox)");
    rps = rng_throughput<real, vex::random::philox>(ctx, N, M);
    prof.toc("OpenCL (philox)");

    std::cout
        << "    OpenCL (philox):   " << rps << std::endl;

    if (options.bm_cpu) {
        std::mt19937 rng( std::rand() );
        std::uniform_real_distribution<real> rnd(0.0, 1.0);

        prof.tic_cpu("C++ (mt19937)");
        real s = 0;
        for(size_t j = 0; j < N; j++)
            s = std::max(s, rnd(rng));
        double time_elapsed = prof.toc("C++ (mt19937)");

        std::cout
            << "    C++    (mt19937):  " << N / time_elapsed << std::endl;
    }

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

//---------------------------------------------------------------------------
template <typename real>
void benchmark_sort(
        const vex::Context &ctx, vex::profiler<> &prof
        )
{
    const size_t N = 16 * 1024 * 1024;
    const size_t M = 16;

    typedef typename std::conditional<
        std::is_same<float, real>::value, cl_uint, cl_ulong
        >::type key_type;

    std::default_random_engine rng( std::rand() );
    std::uniform_int_distribution<key_type> rnd;

    std::vector<key_type> x0(N);
    std::vector<key_type> x1(N);

    std::generate(x0.begin(), x0.end(), [&]() { return rnd(rng); });

    vex::vector<key_type> X0(ctx, x0);
    vex::vector<key_type> X1(ctx, N);

    X1 = X0;
    vex::sort(X1);

    double tot_time = 0;
    for(size_t i = 0; i < M; i++) {
        X1 = X0;
        ctx.finish();
        prof.tic_cpu("VexCL");
        vex::sort(X1);
        ctx.finish();
        tot_time += prof.toc("VexCL");
    }

    std::cout
        << "Sort (" << vex::type_name<key_type>() << ")\n"
        << "    VexCL:         " << N * M / tot_time << " keys/sec\n";

#ifdef VEXCL_HAVE_BOOST_COMPUTE
    X1 = X0;
    vex::compute::sort(X1);

    tot_time = 0;
    for(size_t i = 0; i < M; i++) {
        X1 = X0;
        ctx.finish();
        prof.tic_cpu("Boost.Compute");
        vex::compute::sort(X1);
        ctx.finish();
        tot_time += prof.toc("Boost.Compute");
    }

    std::cout
        << "    Boost.Compute: " << N * M / tot_time << " keys/sec\n";
#endif

#ifdef VEXCL_HAVE_CLOGS
    X1 = X0;
    vex::clogs::sort(X1);

    tot_time = 0;
    for(size_t i = 0; i < M; i++) {
        X1 = X0;
        ctx.finish();
        prof.tic_cpu("CLOGS");
        vex::clogs::sort(X1);
        ctx.finish();
        tot_time += prof.toc("CLOGS");
    }

    std::cout
        << "    CLOGS:         " << N * M / tot_time << " keys/sec\n";
#endif

    if (options.bm_cpu) {
        tot_time = 0;
        for(size_t i = 0; i < M; i++) {
            std::copy(x0.begin(), x0.end(), x1.begin());
            prof.tic_cpu("STL");
            std::sort(x1.begin(), x1.end());
            tot_time += prof.toc("STL");
        }

        std::cout << "    STL:           " << N * M / tot_time << " keys/sec\n";
    }

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

//---------------------------------------------------------------------------
template <typename real>
void benchmark_scan(
        const vex::Context &ctx, vex::profiler<> &prof
        )
{
    const size_t N = 16 * 1024 * 1024;
    const size_t M = 16;

    typedef typename std::conditional<
        std::is_same<float, real>::value, cl_uint, cl_ulong
        >::type key_type;

    std::default_random_engine rng( std::rand() );
    std::uniform_int_distribution<key_type> rnd;

    std::vector<key_type> x0(N);
    std::vector<key_type> x1(N);

    std::generate(x0.begin(), x0.end(), [&]() { return rnd(rng); });

    vex::vector<key_type> X0(ctx, x0);
    vex::vector<key_type> X1(ctx, N);

    vex::exclusive_scan(X0, X1);

    ctx.finish();
    prof.tic_cpu("VexCL");

    for(size_t i = 0; i < M; i++)
        vex::exclusive_scan(X0, X1);

    ctx.finish();
    double tot_time = prof.toc("VexCL");

    std::cout
        << "Scan (" << vex::type_name<key_type>() << ")\n"
        << "    VexCL:         " << N * M / tot_time << " keys/sec\n";

#ifdef VEXCL_HAVE_BOOST_COMPUTE
    vex::compute::exclusive_scan(X0, X1);

    ctx.finish();
    prof.tic_cpu("Boost.Compute");

    for(size_t i = 0; i < M; i++)
        vex::compute::exclusive_scan(X0, X1);

    ctx.finish();
    tot_time = prof.toc("Boost.Compute");

    std::cout
        << "    Boost.Compute: " << N * M / tot_time << " keys/sec\n";
#endif

#ifdef VEXCL_HAVE_CLOGS
    vex::clogs::exclusive_scan(X0, X1);

    ctx.finish();
    prof.tic_cpu("CLOGS");

    for(size_t i = 0; i < M; i++)
        vex::clogs::exclusive_scan(X0, X1);

    ctx.finish();
    tot_time = prof.toc("CLOGS");

    std::cout
        << "    CLOGS:         " << N * M / tot_time << " keys/sec\n";
#endif

    if (options.bm_cpu) {
        prof.tic_cpu("CPU");
        for(size_t i = 0; i < M; i++) {
            key_type sum = key_type();
            for(size_t j = 0; j < N; ++j) {
                key_type next = sum + x0[j];
                x1[j] = sum;
                sum = next;
            }
        }
        tot_time = prof.toc("CPU");

        std::cout << "    CPU:           " << N * M / tot_time << " keys/sec\n";
    }

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

//---------------------------------------------------------------------------
template <typename real>
void run_tests(const vex::Context &ctx, vex::profiler<> &prof)
{
    std::cout
        << "----------------------------------------------------------\n"
        << "Profiling \"" << vex::type_name<real>() << "\" performance\n"
        << "----------------------------------------------------------\n"
        << ctx << std::endl;

    std::ostringstream fname;
    fname << "profile_" << vex::type_name<real>() << ".dat";
    std::ofstream log(fname.str().c_str(), std::ios::app);

    log << ctx.size() << " ";

    double gflops, bwidth;

    prof.tic_cpu( vex::type_name<real>() );

    if (options.bm_saxpy) {
        prof.tic_cpu("Vector SAXPY");
        std::tie(gflops, bwidth) = benchmark_saxpy<real>(ctx, prof);
        prof.toc("Vector SAXPY");

        log << gflops << " " << bwidth << " ";
    }

    if (options.bm_vector) {
        prof.tic_cpu("Vector arithmetic");
        std::tie(gflops, bwidth) = benchmark_vector<real>(ctx, prof);
        prof.toc("Vector arithmetic");

        log << gflops << " " << bwidth << " ";
    }

    if (options.bm_reductor) {
        prof.tic_cpu("Reduction");
        std::tie(gflops, bwidth) = benchmark_reductor<real>(ctx, prof);
        prof.toc("Reduction");

        log << gflops << " " << bwidth << " ";
    }

    if (options.bm_stencil) {
        prof.tic_cpu("Stencil");
        std::tie(gflops, bwidth) = benchmark_stencil<real>(ctx, prof);
        prof.toc("Stencil");

        log << gflops << " " << bwidth << " ";
    }

    if (options.bm_spmv) {
        prof.tic_cpu("SpMV");
        std::tie(gflops, bwidth) = benchmark_spmv<real>(ctx, prof);
        prof.toc("SpMV");

        log << gflops << " " << bwidth << std::endl;

        prof.tic_cpu("SpMV (CCSR)");
        std::tie(gflops, bwidth) = benchmark_spmv_ccsr<real>(ctx, prof);
        prof.toc("SpMV (CCSR)");
    }

    if (options.bm_rng) {
        prof.tic_cpu("Random number generation");
        benchmark_rng<real>(ctx, prof);
        prof.toc("Random number generation");
    }

    if (options.bm_sort) {
        prof.tic_cpu("Sorting");
        benchmark_sort<real>(ctx, prof);
        prof.toc("Sorting");
    }

    if (options.bm_scan) {
        prof.tic_cpu("Scanning");
        benchmark_scan<real>(ctx, prof);
        prof.toc("Scanning");
    }

    prof.toc( vex::type_name<real>() );

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

//---------------------------------------------------------------------------
int main(int argc, char *argv[]) {
    namespace po = boost::program_options;
    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "show help")
        ("revert,r", "revert options")
        ("bm_saxpy",
            po::value<bool>(&options.bm_saxpy)->default_value(true),
            "benchmark SAXPY (on/off)"
            )
        ("bm_vec",
            po::value<bool>(&options.bm_vector)->default_value(true),
            "benchmark vector arithmetics (on/off)"
            )
        ("bm_rdc",
            po::value<bool>(&options.bm_reductor)->default_value(true),
            "benchmark reduction (on/off)"
            )
        ("bm_stn",
            po::value<bool>(&options.bm_stencil)->default_value(true),
            "benchmark stencil convolution (on/off)"
            )
        ("bm_spm",
            po::value<bool>(&options.bm_spmv)->default_value(true),
            "benchmark sparse matrix - vector product (on/off)"
            )
        ("bm_rng",
            po::value<bool>(&options.bm_rng)->default_value(true),
            "benchmark random number generation (on/off)"
            )
        ("bm_sort",
            po::value<bool>(&options.bm_sort)->default_value(true),
            "benchmark sorting (on/off)"
            )
        ("bm_scan",
            po::value<bool>(&options.bm_sort)->default_value(true),
            "benchmark exclusive scan (on/off)"
            )
        ("bm_cpu",
            po::value<bool>(&options.bm_cpu)->default_value(true),
            "benchmark host CPU performance (on/off)"
            )
        ;
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout << desc << std::endl;
        return 0;
    }

    if (vm.count("revert")) {
        options.revert();
    }

    try {
        vex::profiler<> prof;

        {
            vex::Context ctx(vex::Filter::Env && vex::Filter::DoublePrecision);
            if (ctx) run_tests<double>(ctx, prof);
        }

        {
            vex::Context ctx(vex::Filter::Env);
            if (ctx) run_tests<float>(ctx, prof);
        }

        std::cout << prof << std::endl;
    } catch (const vex::error &e) {
        std::cerr << e << std::endl;
        return 1;
    }
}

// vim: et


#include <iostream>
#include <fstream>

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

#include <imgproc/image_operator.hpp>
#include <imgproc/derivative_gradient.hpp>
#include <edge/nms.hpp>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>
#define WRITE_IMAGE_FILES
using namespace lsfm;
using namespace std;
namespace fs = boost::filesystem;

#ifdef WRITE_IMAGE_FILES
constexpr int runs = 1;
#else
constexpr int runs = 10;
#endif

struct Entry {
    Entry() {}

    Entry(const ImageOperatorPtr& a, const std::string& b)
        : filter(a), name(b) {}

    ImageOperatorPtr filter;
    std::string name;

    inline cv::Mat process(const cv::Mat src) {
        cv::Mat ret;
        filter->apply(src,ret);
        return ret;
    }
};

template<class NMS>
cv::Mat createNMS(NMS &nms) {

    //cv::Mat emap = nms.directionMap();
    cv::Mat emap = nms.hysteresis();
    cv::Mat emapImg;
    emapImg.create(emap.rows, emap.cols, CV_8UC3);

    emapImg.setTo(cv::Vec3b(0, 0, 0));
    emapImg.setTo(cv::Vec3b(220, 150, 255), emap == 7); // magenta2
    emapImg.setTo(cv::Vec3b(255, 0, 150), emap == 6); // lila
    emapImg.setTo(cv::Vec3b(255, 0, 0), emap == 5); // blue
    emapImg.setTo(cv::Vec3b(255, 255, 0), emap == 4); // cyan
    emapImg.setTo(cv::Vec3b(0, 255, 0), emap == 3); // green
    emapImg.setTo(cv::Vec3b(0, 255, 255), emap == 2); // yellow
    emapImg.setTo(cv::Vec3b(0, 150, 255), emap == 1); // orange
    emapImg.setTo(cv::Vec3b(0, 0, 255), emap == 0); // red
    return emapImg;
}

void parseFolder(const fs::path &folder, std::vector<fs::path> &files) {
    fs::directory_iterator end_iter;
    for_each(fs::directory_iterator(folder), fs::directory_iterator(), [&files](const fs::path& file) {
        if (fs::is_regular_file(file))
        {
            std::string ext = file.extension().generic_string();
            boost::algorithm::to_lower(ext);
            if (ext == ".jpg" || ext == ".png") {
                files.push_back(file);
            }
        }
        if (fs::is_directory(file))
            parseFolder(file, files);
    });
}

double processError(Entry &e, const fs::path& path, int n, double &time) {
    std::cout << "processing noise" << n << " (" << e.name <<")...";
    std::vector<fs::path> files;
    parseFolder(path, files);
    GaussianNoiseOperator op_noise(n);
#ifdef WRITE_IMAGE_FILES
    DerivativeGradient<uchar, short, int, float, SobelDerivative, QuadraticMagnitude> sobel;
    NonMaximaSuppression<short, int, float, FastNMS8<short, int, float>> nms;
#endif

    double ret = 0;
    time = 0;
    int count = 0;
    std::for_each(files.begin(), files.end(), [&](const fs::path& file) {
        cv::Mat src = cv::imread(file.generic_string()), src_n;
        if (src.empty())
        {
            cout << "Can not open " << file.generic_string() << endl;
            return;
        }

        cv::cvtColor(src, src, CV_BGR2GRAY);
        src_n = src.clone();
        op_noise(src_n);

        cv::Mat res;
        int64 start;
        for (int i = 0; i != runs; ++i) {
            start = cv::getTickCount();
            res = e.process(src_n);
            time += cv::getTickCount() - start;
        }

#ifdef WRITE_IMAGE_FILES
        std::string name = e.name;
        std::replace(name.begin(), name.end(), ' ', '_');
        cv::imwrite("./denoise/" + file.stem().generic_string() + "_noise" + boost::str(boost::format("%i") % (n)) + "_" + name + ".png", res);
        sobel.process(res);
        nms.process(sobel);
        cv::imwrite("./denoise/" + file.stem().generic_string() + "_noise" + boost::str(boost::format("%i") % (n)) + "_nms_" + name + ".png", createNMS(nms));
#endif
        res.convertTo(res, CV_32S);
        src.convertTo(src, CV_32S);

        ret += sum(abs(src - res))[0] / res.size().area() / 255;
        ++count;
    });
    ret /= count;
    time *= 1000/(count * runs * cv::getTickFrequency());
    std::cout << "time: " <<  time << "ms, error: " << ret << std::endl;
    return ret;
}

int main(int argc, char** argv)
{
    char c;
    std::cin >> c;

    fs::path path = "../../images/MDB/MiddEval3-Q";

    std::vector<Entry> filter;

    filter.push_back(Entry(NoOp::create(), "noop"));

    filter.push_back(Entry(BlurOperator::create(3), "box 3"));
    filter.push_back(Entry(BlurOperator::create(5), "box 5"));
    filter.push_back(Entry(BlurOperator::create(7), "box 7"));
    filter.push_back(Entry(BlurOperator::create(9), "box 9"));

    filter.push_back(Entry(GaussianBlurOperator::create(3), "gauss 3"));
    filter.push_back(Entry(GaussianBlurOperator::create(5), "gauss 5"));
    filter.push_back(Entry(GaussianBlurOperator::create(7), "gauss 7"));
    filter.push_back(Entry(GaussianBlurOperator::create(9), "gauss 9"));

    filter.push_back(Entry(MedianBlurOperator::create(3), "median 3"));
    filter.push_back(Entry(MedianBlurOperator::create(5), "median 5"));
    filter.push_back(Entry(MedianBlurOperator::create(7), "median 7"));
    filter.push_back(Entry(MedianBlurOperator::create(9), "median 9"));

    filter.push_back(Entry(BilateralOperator::create(3), "bilat 3"));
    filter.push_back(Entry(BilateralOperator::create(5), "bilat 5"));
    filter.push_back(Entry(BilateralOperator::create(7), "bilat 7"));
    filter.push_back(Entry(BilateralOperator::create(9), "bilat 9"));

    filter.push_back(Entry(FastNlMeansOperator::create(5), "nlmean 5"));
    filter.push_back(Entry(FastNlMeansOperator::create(10), "nlmean 10"));
    filter.push_back(Entry(FastNlMeansOperator::create(15), "nlmean 15"));
    filter.push_back(Entry(FastNlMeansOperator::create(20), "nlmean 20"));

    int rows = filter.size() + 1;
    int cols = 11;
    std::vector<std::vector<std::string>> table;
    table.resize(rows);
    for_each(table.begin(), table.end(), [&](std::vector<std::string> &col) {
        col.resize(cols);
    });

    table[0][0] = "Method";
    table[0][1] = "n10 error";
    table[0][2] = "n20 error";
    table[0][3] = "n30 error";
    table[0][4] = "n40 error";
    table[0][5] = "n50 error";
    table[0][6] = "n10 time";
    table[0][7] = "n20 time";
    table[0][8] = "n30 time";
    table[0][9] = "n40 time";
    table[0][10] = "n50 time";

    int row = 1;
    for_each(filter.begin(), filter.end(), [&](Entry &e) {
        table[row++][0] = e.name;
    });

    for (int col = 1; col != 6; ++col) {
        int row = 1;
        for_each(filter.begin(), filter.end(), [&](Entry &e) {
            double time;
            table[row][col] = boost::str(boost::format("%.3f") % (processError(e, path, 10 * col, time)));
            table[row++][col+5] = boost::str(boost::format("%.3f") % (time));
        });
    }

    std::ofstream ofs;
    ofs.open("image_denoise.csv");

    for_each(table.begin(), table.end(), [&](const std::vector<std::string> &col) {
        for_each(col.begin(), col.end(), [&](const std::string &cell) {
            std::cout << cell << "\t";
            ofs << cell << ";";
        });
        std::cout << std::endl;
        ofs << std::endl;
    });

    ofs.close();

    return 0;
}


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

Copyright (c) 2017 Bob Perry / asw204k_AT_yahoo.com
All rights reserved.

Please see license in the project root directory fro more details

*/

#include "oxapp.h"
#include "algo/linregres.h"
#include <iostream>
#include <cassert>

#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/thread/thread.hpp>

using namespace std;

int main(int argc, char *argv[] ) {

  LinRegres l(300); // 300 ms time window

  int count = 0;

  for( int i = 0; i < 1000; i = i + 100 ) {
    OxApp::algo_press->set_val( TE_ALTITUDE ,i);
    cerr << l.update(OxApp::algo_press->get_time( TE_ALTITUDE ),
                     OxApp::algo_press->get_val( TE_ALTITUDE )) << " " <<
      l.get_depth() << endl;

    b::this_thread::sleep(b::posix_time::milliseconds(100));
    if( l.ready() ) {
      count += 1;

    }
  }
  assert( count == 7);
  assert ( l.slope() < 1.0 );
  assert ( l.slope() > 0.0 );

  count = 0;

  for( int i = 1000; i >= 0; i = i - 100 ) {
    OxApp::algo_press->set_val( TE_ALTITUDE ,i);
    cerr << l.update(OxApp::algo_press->get_time( TE_ALTITUDE ),
                     OxApp::algo_press->get_val( TE_ALTITUDE )) << " " <<
      l.get_depth() << endl;

    b::this_thread::sleep(b::posix_time::milliseconds(100));
    if( l.ready() ) {
      count += 1;

    }
  }

  assert( count == 11);
  assert ( l.slope() > -1.0 );
  assert ( l.slope() < 0.0 );

  return 0;
}


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * 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.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE histogramnew_test

// Standard includes
#include <exception>
#include <iostream>

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/tools/histogramnew.h"
#include "votca/tools/table.h"

using namespace std;
using namespace votca::tools;

BOOST_AUTO_TEST_SUITE(histogramnew_test)

BOOST_AUTO_TEST_CASE(create_test) { HistogramNew hn; }

BOOST_AUTO_TEST_CASE(init_test) {
  HistogramNew hn;
  double min_v = 1.2;
  double max_v = 102.0;
  hn.Initialize(min_v, max_v, 10);
}

BOOST_AUTO_TEST_CASE(step_test) {
  HistogramNew hn;
  double min_v = 1;
  double max_v = 9;
  hn.Initialize(min_v, max_v, 8);
  double step = hn.getStep();
  votca::Index value = static_cast<votca::Index>(step);
  BOOST_CHECK_EQUAL(value * 10, 10);
}

BOOST_AUTO_TEST_CASE(nbins_test) {
  HistogramNew hn;
  double min_v = 1.0;
  double max_v = 9.0;
  hn.Initialize(min_v, max_v, 8);
  votca::Index bins = hn.getNBins();
  BOOST_CHECK_EQUAL(bins * 10, 80);
}

BOOST_AUTO_TEST_CASE(Process_test) {
  HistogramNew hn;
  double min_v = 0.0;
  double max_v = 10.0;
  hn.Initialize(min_v, max_v, 11);
  vector<double> data;
  for (double x = 0; x < 10; ++x) {
    data.push_back(x);
  }
  hn.ProcessRange(data.begin(), data.end());
  hn.Process(4.5);
  BOOST_CHECK_EQUAL(static_cast<votca::Index>(hn.getStep() * 10), 10);
  auto dat = hn.data();
  // Range -0.5 - 0.5
  BOOST_CHECK_EQUAL(static_cast<votca::Index>(dat.y(0)), 1);
  // Range 0.5 - 1.5
  BOOST_CHECK_EQUAL(static_cast<votca::Index>(dat.y(1)), 1);
  // Range 1.5 - 2.5
  BOOST_CHECK_EQUAL(static_cast<votca::Index>(dat.y(2)), 1);
  // Range 2.5 - 3.5
  BOOST_CHECK_EQUAL(static_cast<votca::Index>(dat.y(3)), 1);
  // Range 3.5 - 4.5
  BOOST_CHECK_EQUAL(static_cast<votca::Index>(dat.y(4)), 1);
  // Range 4.5 - 5.5
  BOOST_CHECK_EQUAL(static_cast<votca::Index>(dat.y(5)), 2);
  // Range 5.5 - 6.5
  BOOST_CHECK_EQUAL(static_cast<votca::Index>(dat.y(6)), 1);
  // Range 6.5 - 7.5
  BOOST_CHECK_EQUAL(static_cast<votca::Index>(dat.y(7)), 1);
  // Range 7.5 - 8.5
  BOOST_CHECK_EQUAL(static_cast<votca::Index>(dat.y(8)), 1);
  // Range 8.5 - 9.5
  BOOST_CHECK_EQUAL(static_cast<votca::Index>(dat.y(9)), 1);
  // Range 9.5 - 10.5
  BOOST_CHECK_EQUAL(static_cast<votca::Index>(dat.y(10)), 0);
}

BOOST_AUTO_TEST_CASE(minmax_test) {
  HistogramNew hn;
  double min_v = 0.0;
  double max_v = 10.0;
  hn.Initialize(min_v, max_v, 10);
  vector<double> data;
  for (double x = 0; x < 9; ++x) {
    data.push_back(x);
  }
  hn.ProcessRange(data.begin(), data.end());
  hn.Process(4.5);
  BOOST_CHECK_EQUAL(static_cast<votca::Index>(hn.getMinBinVal()), 0);
  BOOST_CHECK_EQUAL(static_cast<votca::Index>(hn.getMaxBinVal()), 2);
}

BOOST_AUTO_TEST_SUITE_END()


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>

TEST(MathFunctions, if_else) {
  using stan::math::if_else;
  unsigned int c = 5;
  double x = 1.0;
  double y = -1.0;
  EXPECT_FLOAT_EQ(x, if_else(c, x, y));
  c = 0;
  EXPECT_FLOAT_EQ(y, if_else(c, x, y));

  bool d = true;
  int u = 1;
  int v = -1;
  EXPECT_EQ(1.0, stan::math::if_else(d, u, v));
  d = false;
  EXPECT_FLOAT_EQ(-1.0, stan::math::if_else(d, u, v));

  EXPECT_FLOAT_EQ(1.2, if_else(true, 1.2, 12));
  EXPECT_FLOAT_EQ(12.0, if_else(false, 1.2, 12));

  EXPECT_FLOAT_EQ(1.0, if_else(true, 1, 12.3));
  EXPECT_FLOAT_EQ(12.3, if_else(false, 1, 12.3));
}

TEST(MathFunctions, if_else_promote) {
  using stan::math::if_else;
  double x = 2.5;
  int y = -1;
  EXPECT_FLOAT_EQ(2.5, if_else(true, x, y));
  EXPECT_FLOAT_EQ(-1.0, if_else(false, x, y));
}

TEST(MathFunctions, if_else_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_FLOAT_EQ(1.2, stan::math::if_else(true, 1.2, nan));
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::if_else(false, 1.2, nan));

  EXPECT_PRED1(boost::math::isnan<double>, stan::math::if_else(true, nan, 2.4));
  EXPECT_FLOAT_EQ(2.4, stan::math::if_else(false, nan, 2.4));

  EXPECT_PRED1(boost::math::isnan<double>, stan::math::if_else(true, nan, nan));
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::if_else(false, nan, nan));
}


#include "utilities/cv.h"
#include "model/pointcloud.h"
#include <cfloat>
#include <Eigen/Dense>
#include <QDebug>
#include <vector>
#include <memory>
//#include "plugins/visualisedepth/gridsearch.h"

boost::shared_ptr<std::vector<int>> makeLookup(boost::shared_ptr<PointCloud> cloud) {
    int size = cloud->scan_width() * cloud->scan_height();
    auto grid_to_cloud = boost::make_shared<std::vector<int>>(size, -1);
    for(uint i = 0; i < cloud->size(); i++) {
        int grid_idx = cloud->cloudToGridMap()[i];
        (*grid_to_cloud)[grid_idx] = i;
    }
    return grid_to_cloud;
}

boost::shared_ptr<std::vector<float>> makeDistmap(
        boost::shared_ptr<PointCloud> cloud,
        boost::shared_ptr<std::vector<float>> distmap) {
    uint size = cloud->scan_width() * cloud->scan_height();

    if(distmap == nullptr || distmap->size() != size)
        distmap = boost::make_shared<std::vector<float>>(size, 0.0f);

    float max_dist = 0.0;

    for(uint i = 0; i < cloud->size(); i++) {
        int grid_idx = cloud->cloudToGridMap()[i];
        pcl::PointXYZI & p = (*cloud)[i];
        (*distmap)[grid_idx] = sqrt(pow(p.x, 2) + pow(p.y, 2) + pow(p.z, 2));

        if((*distmap)[i] > max_dist)
            max_dist = (*distmap)[i];
    }

    return distmap;
}

static const double sobel_x[9] = {
    1, 0, -1,
    2, 0, -2,
    1, 0, -1,
};

static const double sobel_y[9] = {
    1, 2, 1,
    0, 0, 0,
    -1, -2, -1,
};

boost::shared_ptr<std::vector<float> > gradientImage(boost::shared_ptr<std::vector<float>> image,
        int w, int h,
        boost::shared_ptr<std::vector<float>> out_image) {

    uint size = w*h;

    if(out_image == nullptr || out_image->size() != size) {
        out_image = boost::make_shared<std::vector<float>>(size, 0);
    }

    float * grad_mag = &out_image->at(0);

    // Calculate the gradient magnitude
    for(int x = 0; x < w; x++){
        for(int y = 0; y < h; y++){
            float gx = convolve_op(w, h, &(*image)[0], x, y, sobel_x, 3);
            float gy = convolve_op(w, h, &(*image)[0], x, y, sobel_y, 3);
            grad_mag[x+y*w] = sqrt(gx*gx + gy*gy);
        }
    }

    return out_image;
}

boost::shared_ptr<std::vector<float> > convolve(
        boost::shared_ptr<std::vector<float>> image,
        int w, int h, const double * filter, const int filter_size,
        boost::shared_ptr<std::vector<float>> out_image) {

    uint size = w*h;
    if(out_image == nullptr || out_image->size() != size) {
        out_image = boost::make_shared<std::vector<float>>(size, 0);
    }

    float * img = &out_image->at(0);

    for(int x = 0; x < w; x++){
        for(int y = 0; y < h; y++){
            img[x+y*w] = convolve_op(w, h, &(*image)[0], x, y, filter, filter_size);
        }
    }

    return out_image;
}

boost::shared_ptr<std::vector<float> > morphology(boost::shared_ptr<std::vector<float>> image,
        int w, int h, const int *strct, int strct_size,
        Morphology type,
        boost::shared_ptr<std::vector<float>> out_image) {

    uint size = w*h;
    if(out_image == nullptr || out_image->size() != size) {
        out_image = boost::make_shared<std::vector<float>>(size, 0);
    }

    //float * img = &out_image->at(0);

    // Calculate the gradient magnitude
    for(int x = 0; x < w; x++){
        for(int y = 0; y < h; y++){
            morph_op(&(*image)[0], w, h, &(*out_image)[0], x, y, strct, strct_size, type);
        }
    }

    return out_image;
}

// stdev on depth image
boost::shared_ptr<std::vector<float> > stdev(
        boost::shared_ptr<std::vector<float>> image,
        int w, int h, const int local_size,
        boost::shared_ptr<std::vector<float>> out_image) {

    uint size = w*h;
    if(out_image == nullptr || out_image->size() != size) {
        out_image = boost::make_shared<std::vector<float>>(size, 0);
    }

    float * img = &out_image->at(0);

    for(int x = 0; x < w; x++){
        for(int y = 0; y < h; y++){
            img[x+y*w] = stdev_op(w, h, &(*image)[0], x, y, local_size);
        }
    }

    return out_image;
}

boost::shared_ptr<std::vector<float> > interpolate(
        boost::shared_ptr<std::vector<float>> image,
        int w, int h, const int nsize,
        boost::shared_ptr<std::vector<float>> out_image) {

    uint size = w*h;
    if(out_image == nullptr || out_image->size() != size) {
        out_image = boost::make_shared<std::vector<float>>(size, 0);
    }

    //float * img = &out_image->at(0);

    // Calculate the gradient magnitude
    for(int x = 0; x < w; x++){
        for(int y = 0; y < h; y++){
            interp_op(&(*image)[0], w, h, &(*out_image)[0], x, y, nsize);
        }
    }

    return out_image;
}

boost::shared_ptr<std::vector<float> > stdev_dist(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,
                            const double radius, int max_nn, bool use_depth) {

    boost::shared_ptr<std::vector<float>> stdevs
                        = boost::make_shared<std::vector<float>>(cloud->size());

    std::vector<int> idxs(0);
    std::vector<float> dists(0);

    // 1m radius
    //const double radius = 1.0;

    //GridSearch gs(*cloud);

    // center
    Eigen::Map<Eigen::Vector3f> center(cloud->sensor_origin_.data());

    double resolution = 0.02;
    Octree::Ptr octree = Octree::Ptr(new Octree(resolution));
    octree->setInputCloud(cloud);
    octree->defineBoundingBox();
    octree->addPointsFromInputCloud();

    //const Octree::Ptr ot = cloud->octree();
    for(uint i = 0; i < cloud->size(); i++){
        idxs.clear();
        dists.clear();

        octree->radiusSearch(cloud->points[i], radius, idxs, dists, max_nn);
        //grid_nn_op(i, *cloud, idxs, 1, 50);
        //gs.radiusSearch((*cloud)[i], radius, idxs, dists, max_nn);

        // calculate stdev of the distances?
        // bad idea because you have a fixed radius
        // Calculate distance from center of scan

        Eigen::Map<const Eigen::Vector3f> query_point(&(cloud->points[i].x));

        float sum = 0.0f;
        float sum_sq = 0.0f;

        for(int idx : idxs) {
            const float * data = &(cloud->points[idx].x);
            Eigen::Map<const Eigen::Vector3f> point(data);

            float dist;

            if(use_depth)
                dist = (point-center).norm();
            else
                dist = (point-query_point).norm();

            sum += dist;
            sum_sq += dist*dist;
        }

        if(idxs.size() > 2)
            (*stdevs)[i] = (sum_sq - (sum*sum)/(idxs.size()))/((idxs.size())-1);
        else
            (*stdevs)[i] = 0;

    }

    return stdevs;
}

boost::shared_ptr<std::vector<float>> cloudToGrid(const std::vector<int> &map,
        uint img_size,
        boost::shared_ptr<std::vector<float>> input,
        boost::shared_ptr<std::vector<float>> img) {

    if(img == nullptr || img->size() != img_size)
        img = boost::make_shared<std::vector<float>>(img_size, 0.0f);

    for(uint i = 0; i < map.size(); i++) {
        int grid_idx = map[i];
        (*img)[grid_idx] = (*input)[i];
    }

    return img;
}

boost::shared_ptr<std::vector<Eigen::Vector3f> > getHist(boost::shared_ptr<PointCloud> cloud, double radius, uint max_nn) {

    boost::shared_ptr<std::vector<Eigen::Vector3f> > eigen_vals =
            boost::make_shared<std::vector<Eigen::Vector3f>>(cloud->size());

    pcl::KdTreeFLANN<pcl::PointXYZI> search;
    search.setInputCloud(pcl::PointCloud<pcl::PointXYZI>::ConstPtr(cloud.get(), boost::serialization::null_deleter()));

    QTime total;
    total.start();

    int less_than_three_points_count = 0;

    boost::shared_ptr <std::vector<int> > kIdxs;
    kIdxs = boost::shared_ptr <std::vector<int> >(new std::vector<int>);
    std::vector<float> kDist;

    float min = FLT_MAX;
    float max = FLT_MIN;

    // For every point
    for(unsigned int i = 0; i < cloud->size(); i++){

        search.radiusSearch(i, radius, *kIdxs, kDist, max_nn);

        if(kIdxs->size() > max_nn){
            qDebug() << "Whoops! Too many";
            continue;
        }

        if(kIdxs->size() < 3) {
            less_than_three_points_count++;
            (*eigen_vals)[i] = Eigen::Vector3f(0, 0, 1.0f); // Assume isolated point
            continue;
        }

        pcl::PCA<pcl::PointXYZI> pcEstimator(true);
        pcl::PointCloud<pcl::PointXYZI>::ConstPtr const_cloud(cloud.get(), boost::serialization::null_deleter());
        pcEstimator.setInputCloud (const_cloud);
        pcEstimator.setIndices(kIdxs);
        (*eigen_vals)[i] = pcEstimator.getEigenValues();

        (*eigen_vals)[i].normalize(); // SHOULD THIS BE NORMALISED?

    }

    /*
    for(unsigned int i = 0; i < cloud->size(); i++){
        (*eigen_vals)[i] = ((*eigen_vals)[i] - Eigen::Vector3f(min, min, min) ) / (max - min);
    }
    */

    qDebug() << "Radius: " << radius << " Max_nn: " << max_nn << " Time: " << total.elapsed()/1000.0f << "Sec";
    qDebug("Points with less than %d neighbours: %d", max_nn, less_than_three_points_count);
    qDebug("Max: %f, Min: %f", max, min);

    return eigen_vals;
}


#include <gtest/gtest.h>

#include <TVectorD.h>
#include <Eigen/Dense>

#include <RootEigenTransformations.h>

namespace genfit {

    class RootEigenTransformations : public ::testing::Test {
    protected:
    };

    TEST_F(RootEigenTransformations, VectorTransformation) {
        TVectorD myTVectorD(5);
        myTVectorD(0) = 0.5;
        myTVectorD(0) = 0.4;
        myTVectorD(0) = 0.3;
        myTVectorD(0) = 0.2;
        myTVectorD(0) = 0.1;

        const auto myEigenVector(rootVectorToEigenVector<5>(myTVectorD));
        EXPECT_EQ(myTVectorD(0), myEigenVector(0));
        EXPECT_EQ(myTVectorD(1), myEigenVector(1));
        EXPECT_EQ(myTVectorD(2), myEigenVector(2));
        EXPECT_EQ(myTVectorD(3), myEigenVector(3));
        EXPECT_EQ(myTVectorD(4), myEigenVector(4));

        const auto myRootVector(eigenVectorToRootVector<5>(myEigenVector));
        EXPECT_EQ(myTVectorD(0), myRootVector(0));
        EXPECT_EQ(myTVectorD(1), myRootVector(1));
        EXPECT_EQ(myTVectorD(2), myRootVector(2));
        EXPECT_EQ(myTVectorD(3), myRootVector(3));
        EXPECT_EQ(myTVectorD(4), myRootVector(4));
    }

    TEST_F(RootEigenTransformations, SymmetricMatrixTransformation) {
        TMatrixDSym myTMatrixDSym(5);
        for (unsigned int row=0; row<5; ++row) {
            for (unsigned int col=0; col<5; ++col) {
                myTMatrixDSym(row, col) = row*col;  // Put some non-zero value here.
            }
        }

        const auto myEigenMatrix(rootMatrixSymToEigenMatrix<5>(myTMatrixDSym));
        EXPECT_EQ(myTMatrixDSym(0, 0), myEigenMatrix(0, 0));
        EXPECT_EQ(myTMatrixDSym(0, 1), myEigenMatrix(0, 1));
        EXPECT_EQ(myTMatrixDSym(0, 2), myEigenMatrix(0, 2));
        EXPECT_EQ(myTMatrixDSym(0, 3), myEigenMatrix(0, 3));
        EXPECT_EQ(myTMatrixDSym(0, 4), myEigenMatrix(0, 4));
        EXPECT_EQ(myTMatrixDSym(1, 0), myEigenMatrix(1, 0));
        EXPECT_EQ(myTMatrixDSym(1, 1), myEigenMatrix(1, 1));
        EXPECT_EQ(myTMatrixDSym(1, 2), myEigenMatrix(1, 2));
        EXPECT_EQ(myTMatrixDSym(1, 3), myEigenMatrix(1, 3));
        EXPECT_EQ(myTMatrixDSym(1, 4), myEigenMatrix(1, 4));
        EXPECT_EQ(myTMatrixDSym(2, 0), myEigenMatrix(2, 0));
        EXPECT_EQ(myTMatrixDSym(2, 1), myEigenMatrix(2, 1));
        EXPECT_EQ(myTMatrixDSym(2, 2), myEigenMatrix(2, 2));
        EXPECT_EQ(myTMatrixDSym(2, 3), myEigenMatrix(2, 3));
        EXPECT_EQ(myTMatrixDSym(2, 4), myEigenMatrix(2, 4));
        EXPECT_EQ(myTMatrixDSym(3, 0), myEigenMatrix(3, 0));
        EXPECT_EQ(myTMatrixDSym(3, 1), myEigenMatrix(3, 1));
        EXPECT_EQ(myTMatrixDSym(3, 2), myEigenMatrix(3, 2));
        EXPECT_EQ(myTMatrixDSym(3, 3), myEigenMatrix(3, 3));
        EXPECT_EQ(myTMatrixDSym(3, 4), myEigenMatrix(3, 4));
        EXPECT_EQ(myTMatrixDSym(4, 0), myEigenMatrix(4, 0));
        EXPECT_EQ(myTMatrixDSym(4, 1), myEigenMatrix(4, 1));
        EXPECT_EQ(myTMatrixDSym(4, 2), myEigenMatrix(4, 2));
        EXPECT_EQ(myTMatrixDSym(4, 3), myEigenMatrix(4, 3));
        EXPECT_EQ(myTMatrixDSym(4, 4), myEigenMatrix(4, 4));

        const auto myRootMatrix(eigenMatrixToRootMatrixSym<5>(myEigenMatrix));
        EXPECT_EQ(myTMatrixDSym(0, 0), myRootMatrix(0, 0));
        EXPECT_EQ(myTMatrixDSym(0, 1), myRootMatrix(0, 1));
        EXPECT_EQ(myTMatrixDSym(0, 2), myRootMatrix(0, 2));
        EXPECT_EQ(myTMatrixDSym(0, 3), myRootMatrix(0, 3));
        EXPECT_EQ(myTMatrixDSym(0, 4), myRootMatrix(0, 4));
        EXPECT_EQ(myTMatrixDSym(1, 0), myRootMatrix(1, 0));
        EXPECT_EQ(myTMatrixDSym(1, 1), myRootMatrix(1, 1));
        EXPECT_EQ(myTMatrixDSym(1, 2), myRootMatrix(1, 2));
        EXPECT_EQ(myTMatrixDSym(1, 3), myRootMatrix(1, 3));
        EXPECT_EQ(myTMatrixDSym(1, 4), myRootMatrix(1, 4));
        EXPECT_EQ(myTMatrixDSym(2, 0), myRootMatrix(2, 0));
        EXPECT_EQ(myTMatrixDSym(2, 1), myRootMatrix(2, 1));
        EXPECT_EQ(myTMatrixDSym(2, 2), myRootMatrix(2, 2));
        EXPECT_EQ(myTMatrixDSym(2, 3), myRootMatrix(2, 3));
        EXPECT_EQ(myTMatrixDSym(2, 4), myRootMatrix(2, 4));
        EXPECT_EQ(myTMatrixDSym(3, 0), myRootMatrix(3, 0));
        EXPECT_EQ(myTMatrixDSym(3, 1), myRootMatrix(3, 1));
        EXPECT_EQ(myTMatrixDSym(3, 2), myRootMatrix(3, 2));
        EXPECT_EQ(myTMatrixDSym(3, 3), myRootMatrix(3, 3));
        EXPECT_EQ(myTMatrixDSym(3, 4), myRootMatrix(3, 4));
        EXPECT_EQ(myTMatrixDSym(4, 0), myRootMatrix(4, 0));
        EXPECT_EQ(myTMatrixDSym(4, 1), myRootMatrix(4, 1));
        EXPECT_EQ(myTMatrixDSym(4, 2), myRootMatrix(4, 2));
        EXPECT_EQ(myTMatrixDSym(4, 3), myRootMatrix(4, 3));
        EXPECT_EQ(myTMatrixDSym(4, 4), myRootMatrix(4, 4));

    }

    TEST_F(RootEigenTransformations, MatrixTransformation) {
        const unsigned int rows = 7;
        const unsigned int cols = 5;
        TMatrixD myTMatrix(rows, cols);
        for (unsigned int row=0; row<rows; ++row) {
            for (unsigned int col=0; col<cols; ++col) {
                myTMatrix(row, col) = (row+1)*(col*col+1);  // Put some non-zero value here.
            }
        }

        const auto myEigenMatrix(rootMatrixToEigenMatrix<7, 5>(myTMatrix));
        EXPECT_EQ(myTMatrix(0, 0), myEigenMatrix(0, 0));
        EXPECT_EQ(myTMatrix(0, 1), myEigenMatrix(0, 1));
        EXPECT_EQ(myTMatrix(0, 2), myEigenMatrix(0, 2));
        EXPECT_EQ(myTMatrix(0, 3), myEigenMatrix(0, 3));
        EXPECT_EQ(myTMatrix(0, 4), myEigenMatrix(0, 4));
        EXPECT_EQ(myTMatrix(1, 0), myEigenMatrix(1, 0));
        EXPECT_EQ(myTMatrix(1, 1), myEigenMatrix(1, 1));
        EXPECT_EQ(myTMatrix(1, 2), myEigenMatrix(1, 2));
        EXPECT_EQ(myTMatrix(1, 3), myEigenMatrix(1, 3));
        EXPECT_EQ(myTMatrix(1, 4), myEigenMatrix(1, 4));
        EXPECT_EQ(myTMatrix(2, 0), myEigenMatrix(2, 0));
        EXPECT_EQ(myTMatrix(2, 1), myEigenMatrix(2, 1));
        EXPECT_EQ(myTMatrix(2, 2), myEigenMatrix(2, 2));
        EXPECT_EQ(myTMatrix(2, 3), myEigenMatrix(2, 3));
        EXPECT_EQ(myTMatrix(2, 4), myEigenMatrix(2, 4));
        EXPECT_EQ(myTMatrix(3, 0), myEigenMatrix(3, 0));
        EXPECT_EQ(myTMatrix(3, 1), myEigenMatrix(3, 1));
        EXPECT_EQ(myTMatrix(3, 2), myEigenMatrix(3, 2));
        EXPECT_EQ(myTMatrix(3, 3), myEigenMatrix(3, 3));
        EXPECT_EQ(myTMatrix(3, 4), myEigenMatrix(3, 4));
        EXPECT_EQ(myTMatrix(4, 0), myEigenMatrix(4, 0));
        EXPECT_EQ(myTMatrix(4, 1), myEigenMatrix(4, 1));
        EXPECT_EQ(myTMatrix(4, 2), myEigenMatrix(4, 2));
        EXPECT_EQ(myTMatrix(4, 3), myEigenMatrix(4, 3));
        EXPECT_EQ(myTMatrix(4, 4), myEigenMatrix(4, 4));
        EXPECT_EQ(myTMatrix(5, 0), myEigenMatrix(5, 0));
        EXPECT_EQ(myTMatrix(5, 1), myEigenMatrix(5, 1));
        EXPECT_EQ(myTMatrix(5, 2), myEigenMatrix(5, 2));
        EXPECT_EQ(myTMatrix(5, 3), myEigenMatrix(5, 3));
        EXPECT_EQ(myTMatrix(5, 4), myEigenMatrix(5, 4));
        EXPECT_EQ(myTMatrix(6, 0), myEigenMatrix(6, 0));
        EXPECT_EQ(myTMatrix(6, 1), myEigenMatrix(6, 1));
        EXPECT_EQ(myTMatrix(6, 2), myEigenMatrix(6, 2));
        EXPECT_EQ(myTMatrix(6, 3), myEigenMatrix(6, 3));
        EXPECT_EQ(myTMatrix(6, 4), myEigenMatrix(6, 4));

        const auto myRootMatrix(eigenMatrixToRootMatrix<7, 5>(myEigenMatrix));
        EXPECT_EQ(myTMatrix(0, 0), myRootMatrix(0, 0));
        EXPECT_EQ(myTMatrix(0, 1), myRootMatrix(0, 1));
        EXPECT_EQ(myTMatrix(0, 2), myRootMatrix(0, 2));
        EXPECT_EQ(myTMatrix(0, 3), myRootMatrix(0, 3));
        EXPECT_EQ(myTMatrix(0, 4), myRootMatrix(0, 4));
        EXPECT_EQ(myTMatrix(1, 0), myRootMatrix(1, 0));
        EXPECT_EQ(myTMatrix(1, 1), myRootMatrix(1, 1));
        EXPECT_EQ(myTMatrix(1, 2), myRootMatrix(1, 2));
        EXPECT_EQ(myTMatrix(1, 3), myRootMatrix(1, 3));
        EXPECT_EQ(myTMatrix(1, 4), myRootMatrix(1, 4));
        EXPECT_EQ(myTMatrix(2, 0), myRootMatrix(2, 0));
        EXPECT_EQ(myTMatrix(2, 1), myRootMatrix(2, 1));
        EXPECT_EQ(myTMatrix(2, 2), myRootMatrix(2, 2));
        EXPECT_EQ(myTMatrix(2, 3), myRootMatrix(2, 3));
        EXPECT_EQ(myTMatrix(2, 4), myRootMatrix(2, 4));
        EXPECT_EQ(myTMatrix(3, 0), myRootMatrix(3, 0));
        EXPECT_EQ(myTMatrix(3, 1), myRootMatrix(3, 1));
        EXPECT_EQ(myTMatrix(3, 2), myRootMatrix(3, 2));
        EXPECT_EQ(myTMatrix(3, 3), myRootMatrix(3, 3));
        EXPECT_EQ(myTMatrix(3, 4), myRootMatrix(3, 4));
        EXPECT_EQ(myTMatrix(4, 0), myRootMatrix(4, 0));
        EXPECT_EQ(myTMatrix(4, 1), myRootMatrix(4, 1));
        EXPECT_EQ(myTMatrix(4, 2), myRootMatrix(4, 2));
        EXPECT_EQ(myTMatrix(4, 3), myRootMatrix(4, 3));
        EXPECT_EQ(myTMatrix(4, 4), myRootMatrix(4, 4));
        EXPECT_EQ(myTMatrix(5, 0), myRootMatrix(5, 0));
        EXPECT_EQ(myTMatrix(5, 1), myRootMatrix(5, 1));
        EXPECT_EQ(myTMatrix(5, 2), myRootMatrix(5, 2));
        EXPECT_EQ(myTMatrix(5, 3), myRootMatrix(5, 3));
        EXPECT_EQ(myTMatrix(5, 4), myRootMatrix(5, 4));
        EXPECT_EQ(myTMatrix(6, 0), myRootMatrix(6, 0));
        EXPECT_EQ(myTMatrix(6, 1), myRootMatrix(6, 1));
        EXPECT_EQ(myTMatrix(6, 2), myRootMatrix(6, 2));
        EXPECT_EQ(myTMatrix(6, 3), myRootMatrix(6, 3));
        EXPECT_EQ(myTMatrix(6, 4), myRootMatrix(6, 4));

    }

}


#include <cmath>

#include <boost/variant.hpp>

#include "storm/adapters/RationalNumberAdapter.h"
#include "storm/storage/expressions/UnaryNumericalFunctionExpression.h"
#include "storm/storage/expressions/IntegerLiteralExpression.h"
#include "storm/storage/expressions/RationalLiteralExpression.h"
#include "ExpressionVisitor.h"
#include "storm/utility/macros.h"
#include "storm/utility/constants.h"
#include "storm/exceptions/InvalidTypeException.h"
#include "storm/exceptions/InvalidOperationException.h"

namespace storm {
    namespace expressions {
        UnaryNumericalFunctionExpression::UnaryNumericalFunctionExpression(ExpressionManager const& manager, Type const& type, std::shared_ptr<BaseExpression const> const& operand, OperatorType operatorType) : UnaryExpression(manager, type, operand), operatorType(operatorType) {
            // Intentionally left empty.
        }

        UnaryNumericalFunctionExpression::OperatorType UnaryNumericalFunctionExpression::getOperatorType() const {
            return this->operatorType;
        }

        storm::expressions::OperatorType UnaryNumericalFunctionExpression::getOperator() const {
            storm::expressions::OperatorType result = storm::expressions::OperatorType::Minus;
            switch (this->getOperatorType()) {
                case OperatorType::Minus: result = storm::expressions::OperatorType::Minus; break;
                case OperatorType::Floor: result = storm::expressions::OperatorType::Floor; break;
                case OperatorType::Ceil: result = storm::expressions::OperatorType::Ceil; break;
            }
            return result;
        }

        int_fast64_t UnaryNumericalFunctionExpression::evaluateAsInt(Valuation const* valuation) const {
            STORM_LOG_THROW(this->hasIntegerType(), storm::exceptions::InvalidTypeException, "Unable to evaluate expression as integer.");

            if (this->getOperatorType() == OperatorType::Minus) {
                STORM_LOG_THROW(this->getOperand()->hasIntegerType(), storm::exceptions::InvalidTypeException, "Unable to evaluate expression as integer.");
                int_fast64_t result = this->getOperand()->evaluateAsInt(valuation);
                return -result;
            } else {
                // TODO: this should evaluate the operand as a rational.
                double result = this->getOperand()->evaluateAsDouble(valuation);
                switch (this->getOperatorType()) {
                    case OperatorType::Floor: return static_cast<int_fast64_t>(std::floor(result)); break;
                    case OperatorType::Ceil: return static_cast<int_fast64_t>(std::ceil(result)); break;
                    default:
                        STORM_LOG_ASSERT(false, "All other operator types should have been handled before.");
                        return 0;// Warning suppression.
                }
            }
        }

        double UnaryNumericalFunctionExpression::evaluateAsDouble(Valuation const* valuation) const {
            STORM_LOG_THROW(this->hasNumericalType(), storm::exceptions::InvalidTypeException, "Unable to evaluate expression as double.");

            double result = this->getOperand()->evaluateAsDouble(valuation);
            switch (this->getOperatorType()) {
                case OperatorType::Minus: result = -result; break;
                case OperatorType::Floor: result = std::floor(result); break;
                case OperatorType::Ceil: result = std::ceil(result); break;
            }
            return result;
        }

        std::shared_ptr<BaseExpression const> UnaryNumericalFunctionExpression::simplify() const {
            std::shared_ptr<BaseExpression const> operandSimplified = this->getOperand()->simplify();

            if (operandSimplified->isLiteral()) {
                if (operandSimplified->hasIntegerType()) {
                    int_fast64_t value = operandSimplified->evaluateAsInt();
                    switch (this->getOperatorType()) {
                        case OperatorType::Minus:
                            value = -value;
                            break;
                        // Nothing to be done for the other cases:
                        case OperatorType::Floor:
                        case OperatorType::Ceil:
                            break;
                    }
                    return std::shared_ptr<BaseExpression>(new IntegerLiteralExpression(this->getManager(), value));
                } else {
                    storm::RationalNumber value = operandSimplified->evaluateAsRational();
                    bool convertToInteger = false;
                    switch (this->getOperatorType()) {
                        case OperatorType::Minus:
                            value = -value;
                            break;
                        case OperatorType::Floor:
                            value = storm::utility::floor(value);
                            convertToInteger = true;
                            break;
                        case OperatorType::Ceil:
                            value = storm::utility::ceil(value);
                            convertToInteger = true;
                            break;
                    }
                    if (convertToInteger) {
                        return std::shared_ptr<BaseExpression>(new IntegerLiteralExpression(this->getManager(), storm::utility::convertNumber<int64_t>(value)));
                    } else {
                        return std::shared_ptr<BaseExpression>(new RationalLiteralExpression(this->getManager(), value));
                    }
                }
            }

            if (operandSimplified.get() == this->getOperand().get()) {
                return this->shared_from_this();
            } else {
                return std::shared_ptr<BaseExpression>(new UnaryNumericalFunctionExpression(this->getManager(), this->getType(), operandSimplified, this->getOperatorType()));
            }
        }

        boost::any UnaryNumericalFunctionExpression::accept(ExpressionVisitor& visitor, boost::any const& data) const {
            return visitor.visit(*this, data);
        }

        bool UnaryNumericalFunctionExpression::isUnaryNumericalFunctionExpression() const {
            return true;
        }

        void UnaryNumericalFunctionExpression::printToStream(std::ostream& stream) const {
            switch (this->getOperatorType()) {
                case OperatorType::Minus: stream << "-("; break;
                case OperatorType::Floor: stream << "floor("; break;
                case OperatorType::Ceil: stream << "ceil("; break;
            }
            stream << *this->getOperand() << ")";
        }
    }
}


#pragma once
#include <vector>
#include <string>
#include <random>
#include <unordered_set>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <unsupported/Eigen/SparseExtra>

#include "buffalo/algo.hpp"
#include "buffalo/concurrent_queue.hpp"

using namespace std;
using namespace Eigen;

static const int EXP_TABLE_SIZE = 1000;

namespace bpr {

class CBPRMF : public SGDAlgorithm {
public:
    CBPRMF();
    ~CBPRMF();

    void build_exp_table();
    bool parse_option(string opt_path);
    void release();
    bool init(string opt_path);
    void initialize_model(
        float* P, int32_t P_rows,
        float* Q, int32_t Q_rows,
        float* Qb,
        int64_t num_total_samples);
    void worker(int worker_id);
    void set_cumulative_table(int64_t* cum_table, int size);
    void launch_workers();
    void add_jobs(
            int start_x,
            int next_x,
            int64_t* indptr,
            int32_t* positives);
    void wait_until_done();
    void update_parameters();
    double join();
    double compute_loss(
            int32_t num_loss_samples,
            int32_t* users,
            int32_t* positives,
            int32_t* negatives);

    double distance(size_t p, size_t q);

private:
    int64_t* cum_table_;
    int cum_table_size_;
    float exp_table_[EXP_TABLE_SIZE];
};

}


#include <boost/geometry/formulas/elliptic_arc_length.hpp>


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/tuple.hpp>

#include <boost/hana/integral_constant.hpp>

#include <laws/applicative.hpp>
#include <laws/base.hpp>
#include <laws/functor.hpp>
#include <laws/monad.hpp>
#include <laws/monad_plus.hpp>
using namespace boost::hana;

// This test is in its own file to avoid crashing the compiler.

template <int i>
using eq = test::ct_eq<i>;

template <typename ...>
struct F { struct type; };
struct t1; struct t2; struct t3; struct t4;

int main() {
    //////////////////////////////////////////////////////////////////////////
    // Setup for the laws below
    //////////////////////////////////////////////////////////////////////////
    auto eq_tuples = make<Tuple>(
          make<Tuple>()
        , make<Tuple>(eq<0>{})
        , make<Tuple>(eq<0>{}, eq<1>{})
        , make<Tuple>(eq<0>{}, eq<1>{}, eq<2>{}, eq<3>{})
        , make<Tuple>(eq<0>{}, eq<1>{}, eq<2>{}, eq<3>{}, eq<4>{}, eq<5>{})
    );

    auto eq_values = make<Tuple>(eq<0>{}, eq<2>{});

    auto eq_tuples_tuples = make<Tuple>(
          make<Tuple>()
        , make<Tuple>(make<Tuple>(eq<0>{}))
        , make<Tuple>(make<Tuple>(eq<0>{}), make<Tuple>(eq<1>{}, eq<2>{}))
        , make<Tuple>(make<Tuple>(eq<0>{}),
                      make<Tuple>(eq<1>{}, eq<2>{}),
                      make<Tuple>(eq<3>{}, eq<4>{}))
    );

    //////////////////////////////////////////////////////////////////////////
    // Functor
    //////////////////////////////////////////////////////////////////////////
    {
        // Test transform with tuple_t and Metafunctions
        BOOST_HANA_CONSTANT_CHECK(equal(
            transform(tuple_t<>, metafunction<F>),
            tuple_t<>
        ));

        BOOST_HANA_CONSTANT_CHECK(equal(
            transform(tuple_t<t1>, metafunction<F>),
            tuple_t<F<t1>::type>
        ));

        BOOST_HANA_CONSTANT_CHECK(equal(
            transform(tuple_t<t1, t2>, metafunction<F>),
            tuple_t<F<t1>::type, F<t2>::type>
        ));

        BOOST_HANA_CONSTANT_CHECK(equal(
            transform(tuple_t<t1, t2, t3>, metafunction<F>),
            tuple_t<F<t1>::type, F<t2>::type, F<t3>::type>
        ));

        BOOST_HANA_CONSTANT_CHECK(equal(
            transform(tuple_t<t1, t2, t3, t4>, metafunction<F>),
            tuple_t<F<t1>::type, F<t2>::type, F<t3>::type, F<t4>::type>
        ));

        BOOST_HANA_CONSTANT_CHECK(equal(
            transform(tuple_t<t1, t2, t3, t4>, template_<F>),
            tuple_t<F<t1>, F<t2>, F<t3>, F<t4>>
        ));

        test::TestFunctor<Tuple>{eq_tuples, eq_values};
    }

    //////////////////////////////////////////////////////////////////////////
    // Applicative
    //////////////////////////////////////////////////////////////////////////
    {
        test::TestApplicative<Tuple>{};
    }

    //////////////////////////////////////////////////////////////////////////
    // Monad
    //////////////////////////////////////////////////////////////////////////
    {
        test::TestMonad<Tuple>{eq_tuples, eq_tuples_tuples};
    }

    //////////////////////////////////////////////////////////////////////////
    // MonadPlus
    //////////////////////////////////////////////////////////////////////////
    {
        // prepend
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                prepend(long_<0>, tuple_c<long>),
                tuple_c<long, 0>
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                prepend(uint<0>, tuple_c<unsigned int, 1>),
                tuple_c<unsigned int, 0, 1>
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                prepend(llong<0>, tuple_c<long long, 1, 2>),
                tuple_c<long long, 0, 1, 2>
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                prepend(ulong<0>, tuple_c<unsigned long, 1, 2, 3>),
                tuple_c<unsigned long, 0, 1, 2, 3>
            ));
        }

        // empty
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                empty<Tuple>(),
                tuple_c<int>
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                empty<Tuple>(),
                tuple_c<long>
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                empty<Tuple>(),
                tuple_c<void>
            ));
        }

        test::TestMonadPlus<Tuple>{eq_tuples};
    }
}


/*
 *  parse_graph.cpp
 *
 *
 *  Created by Andrea Bedini on 24/Nov/2011.
 *  Copyright (c) 2011-2013, Andrea Bedini <andrea.bedini@gmail.com>.
 *
 *  Distributed under the terms of the Modified BSD License.
 *  The full license is in the file COPYING, distributed as part of
 *  this software.
 *
 */

#include "graph_type.hpp"

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/iterator/counting_iterator.hpp>

#include <sstream>
#include <string>
#include <stdexcept>
#include <vector>

graph_type parse_graph(std::string const& s)
{
  using namespace std;
  using namespace boost;

  vector<pair<int, int> > edge_list;

  string parse_err_msg = "parse error";
  istringstream iss(s);

  if (!iss.good())
    throw runtime_error(parse_err_msg + " 1");

  unsigned int n = 0;
  while (true) {
    unsigned int a = 0, b = 0;

    iss >> a;
    n = max(n, a);

    if (iss.get() !=  '-' or iss.bad())
      throw runtime_error("expecting a dash!");
    if (iss.get() !=  '-' or iss.bad())
      throw runtime_error("expecting a dash!");

    iss >> b;
    n = max(n, b);

    edge_list.push_back(make_pair(a, b));

    if (iss.eof())
      break;

    if (iss.get() !=  ',' or iss.bad())
      throw runtime_error("expecting a comma!");
  }

  boost::counting_iterator<int> ep_iter(0);
  graph_type g(edge_list.begin(), edge_list.end(), ep_iter, n + 1);

  unsigned int i = 0;
  graph_type::vertex_iterator vi, vi_end;
  for (tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    put(vertex_index, g, *vi, i++);

  return g;
}