// This file is distributed under the MIT license.
// See the LICENSE file for details.

#include <common/config.h>

#include <exception>
#include <fstream>
#include <iostream>
#include <memory>
#include <ostream>
#include <string>

#include <boost/filesystem.hpp>

#include <GL/glew.h>

#include <pbrtParser/Scene.h>

#include <Support/CmdLine.h>
#include <Support/CmdLineUtil.h>

#include <visionaray/detail/platform.h>

#include <visionaray/math/io.h>

#include <visionaray/bvh.h>
#include <visionaray/cpu_buffer_rt.h>
#include <visionaray/kernels.h>
#include <visionaray/material.h>
#include <visionaray/scheduler.h>
#include <visionaray/thin_lens_camera.h>

#include <common/manip/arcball_manipulator.h>
#include <common/manip/pan_manipulator.h>
#include <common/manip/zoom_manipulator.h>

#include <common/image.h>
#include <common/make_materials.h>
#include <common/model.h>
#include <common/obj_loader.h>
#include <common/viewer_glut.h>

using namespace visionaray;

using viewer_type = viewer_glut;

//-------------------------------------------------------------------------------------------------
// Phantom Ray-Hair Intersector (Reshetov and Luebke, 2018)
//

//-------------------------------------------------------------------------------------------------
// Ray/cone intersection from appendix A
//

struct RayConeIntersection
{
    inline bool intersect(float r, float dr)
    {
        float r2  = r * r;
        float drr = r * dr;

        float ddd = cd.x * cd.x + cd.y * cd.y;
        dp        = c0.x * c0.x + c0.y * c0.y;
        float cdd = c0.x * cd.x + c0.y * cd.y;
        float cxd = c0.x * cd.y - c0.y * cd.x;

        float c = ddd;
        float b = cd.z * (drr - cdd);
        float cdz2 = cd.z * cd.z;
        ddd += cdz2;
        float a = 2.0f * drr * cdd + cxd * cxd - ddd * r2 + dp * cdz2;

        float discr = b * b - a * c;
        s   = (b - (discr > 0.0f ? sqrtf(discr) : 0.0f)) / c;
        dt  = (s * cd.z - cdd) / ddd;
        dc  = s * s + dp;
        sp  = cdd / cd.z;
        dp += sp * sp;

        return discr > 0.0f;
    }

    vec3  c0;
    vec3  cd;
    float s;
    float dt;
    float dp;
    float dc;
    float sp;
};

inline bool intersectCylinder(basic_ray<float> const& ray, vec3 p0, vec3 p1, float ra)
{
    vec3  ba = p1 - p0;
    vec3  oc = ray.ori - p0;

    float baba = dot(ba, ba);
    float bard = dot(ba, ray.dir);
    float baoc = dot(ba, oc);

    float k2 = baba - bard * bard;
    float k1 = baba * dot(oc, ray.dir) - baoc * bard;
    float k0 = baba * dot(oc, oc) - baoc * baoc - ra * ra * baba;

    float h = k1 * k1 - k2 * k0;

    if (h < 0.0f)
    {
        return false;
    }

    h = sqrtf(h);
    float t = (-k1 - h) / k2;

    // body
    float y = baoc + t * bard;
    if (y > 0.0f && y < baba)
    {
        return true;
    }

    // caps
    t = ((y < 0.0f ? 0.0f : baba) - baoc) / bard;
    if (fabsf(k1 + k2 * t) < h)
    {
        return true;
    }

    return false;
}

//-------------------------------------------------------------------------------------------------
// Curve class
//

struct Curve : primitive<unsigned>
{
    vec3 w0;
    vec3 w1;
    vec3 w2;
    vec3 w3;

    float r;

    vec3 f(float t) const
    {
        float tinv = 1.0f - t;
        return tinv * tinv * tinv * w0
         + 3.0f * tinv * tinv * t * w1
            + 3.0f * tinv * t * t * w2
                      + t * t * t * w3;
    }

    vec3 dfdt(float t) const
    {
        float tinv = 1.0f - t;
        return                 -3.0f * tinv * tinv * w0
         + 3.0f * (3.0f * t * t - 4.0f * t + 1.0f) * w1
                    + 3.0f * (2.0f - 3.0f * t) * t * w2
                                    + 3.0f * t * t * w3;
    }
};

Curve make_curve(vec3 const& w0, vec3 const& w1, vec3 const& w2, vec3 const& w3, float r)
{
    Curve curve;
    curve.w0 = w0;
    curve.w1 = w1;
    curve.w2 = w2;
    curve.w3 = w3;
    curve.r = r;
    return curve;
}

// That's from here: https://www.shadertoy.com/view/MdKBWt
aabb get_bounds(Curve const& curve)
{
    vec3 p0 = curve.w0;
    vec3 p1 = curve.w1;
    vec3 p2 = curve.w2;
    vec3 p3 = curve.w3;

    // extremes
    vec3 mi = min(p0,p3);
    vec3 ma = max(p0,p3);

    // note pascal triangle coefficnets
    vec3 c = -1.0f*p0 + 1.0f*p1;
    vec3 b =  1.0f*p0 - 2.0f*p1 + 1.0f*p2;
    vec3 a = -1.0f*p0 + 3.0f*p1 - 3.0f*p2 + 1.0f*p3;

    vec3 h = b*b - a*c;

    // real solutions
    if (h.x > 0.0f || h.y > 0.0f || h.z > 0.0f)
    {
        vec3 g(
            sqrtf(fabsf(h.x)),
            sqrtf(fabsf(h.y)),
            sqrtf(fabsf(h.z))
            );
        vec3 t1 = clamp((-b - g)/a,vec3(0.0f),vec3(1.0f)); vec3 s1 = 1.0f-t1;
        vec3 t2 = clamp((-b + g)/a,vec3(0.0f),vec3(1.0f)); vec3 s2 = 1.0f-t2;
        vec3 q1 = s1*s1*s1*p0 + 3.0f*s1*s1*t1*p1 + 3.0f*s1*t1*t1*p2 + t1*t1*t1*p3;
        vec3 q2 = s2*s2*s2*p0 + 3.0f*s2*s2*t2*p1 + 3.0f*s2*t2*t2*p2 + t2*t2*t2*p3;

        if (h.x > 0.0f)
        {
            mi.x = min(mi.x,min(q1.x,q2.x));
            ma.x = max(ma.x,max(q1.x,q2.x));
        }
        if (h.y > 0.0f)
        {
            mi.y = min(mi.y,min(q1.y,q2.y));
            ma.y = max(ma.y,max(q1.y,q2.y));
        }
        if (h.z > 0.0f)
        {
            mi.z = min(mi.z,min(q1.z,q2.z));
            ma.z = max(ma.z,max(q1.z,q2.z));
        }
    }

    return aabb(mi - vec3(curve.r), ma + vec3(curve.r));
}

std::pair<Curve, Curve> split(Curve const& curve)
{
    std::pair<Curve, Curve> result;

    vec3 p0 = curve.w0;
    vec3 p1 = curve.w1;
    vec3 p2 = curve.w2;
    vec3 p3 = curve.w3;

    vec3 q0 = (p0 + p1) / 2.0f;
    vec3 q1 = (p1 + p2) / 2.0f;
    vec3 q2 = (p2 + p3) / 2.0f;

    vec3 r0 = (q0 + q1) / 2.0f;
    vec3 r1 = (q1 + q2) / 2.0f;

    vec3 s0 = (r0 + r1) / 2.0f;

    result.first  = make_curve(p0, q0, r0, s0, curve.r);
    result.second = make_curve(s0, r1, q2, p3, curve.r);

    return result;
}

//-------------------------------------------------------------------------------------------------
// Transform entities to ray-centric coordinate system RCC
//

struct TransformToRCC
{
    inline TransformToRCC(basic_ray<float> const& r)
    {
        vec3 e1;
        vec3 e2;
        vec3 e3 = normalize(r.dir);
        make_orthonormal_basis(e1, e2, e3);
        xformInv = mat4(
            vec4(e1,    0.0f),
            vec4(e2,    0.0f),
            vec4(e3,    0.0f),
            vec4(r.ori, 1.0f)
            );
        xform = inverse(xformInv);
    }

    inline vec3 xfmPoint(vec3 point)
    {
        return (xform * vec4(point, 1.0f)).xyz();
    }

    inline vec3 xfmVector(vec3 vector)
    {
        return (xform * vec4(vector, 0.0f)).xyz();
    }

    inline vec3 xfmPointInv(vec3 point)
    {
        return (xformInv * vec4(point, 1.0f)).xyz();
    }

    inline vec3 xfmVectorInv(vec3 vector)
    {
        return (xformInv * vec4(vector, 0.0f)).xyz();
    }

    mat4 xform;
    mat4 xformInv;
};

inline hit_record<basic_ray<float>, primitive<unsigned>> intersect(basic_ray<float> const& r, Curve const& curve)
{
    hit_record<basic_ray<float>, primitive<unsigned>> result;
    result.hit = false;

    // Early exit check against enclosing cylinder
    auto distToCylinder = [&curve](vec3 pt) {
        return length(cross(pt - curve.w0, pt - curve.w3)) / length(curve.w3 - curve.w0);
    };

    // TODO: could compute tighter bounding cylinder than this one!
    float rmax = distToCylinder(curve.f(0.33333f));
    rmax = fmaxf(rmax, distToCylinder(curve.f(0.66667f)));
    rmax += curve.r;

    vec3 axis = normalize(curve.w3 - curve.w0);
    vec3 p0   = curve.w0 - axis * curve.r;
    vec3 p1   = curve.w3 + axis * curve.r;

    if (!intersectCylinder(r, p0, p1, rmax))
    {
        return result;
    }

    // Transform curve to RCC
    TransformToRCC rcc(r);
    Curve xcurve = make_curve(
        rcc.xfmPoint(curve.w0),
        rcc.xfmPoint(curve.w1),
        rcc.xfmPoint(curve.w2),
        rcc.xfmPoint(curve.w3),
        curve.r
        );

    // "Test for convergence. If the intersection is found,
    // report it, otherwise start at the other endpoint."

    // Compute curve end to start at
    float tstart = dot(xcurve.w3 - xcurve.w0, r.dir) > 0.0f ? 0.0f : 1.0f;

    for (int ep = 0; ep < 2; ++ep)
    {
        float t   = tstart;

        RayConeIntersection rci;

        float told = 0.0f;
        float dt1 = 0.0f;
        float dt2 = 0.0f;

        for (int i = 0; i < 40; ++i)
        {
            rci.c0 = xcurve.f(t);
            rci.cd = xcurve.dfdt(t);

            bool phantom = !rci.intersect(curve.r, 0.0f/*cylinder*/);

            // "In all examples in this paper we stop iterations when dt < 5x10^−5"
            if (!phantom && fabsf(rci.dt) < 5e-5f)
            {
                //vec3 n = normalize(curve.dfdt(t));
                rci.s += rci.c0.z;
                result.t = rci.s;
                result.u = t; // abuse param u to store curve's t
                result.hit = true;
                result.isect_pos = r.ori + result.t * r.dir;
                break;
            }

            rci.dt = min(rci.dt, 0.5f);
            rci.dt = max(rci.dt, -0.5f);

            dt1 = dt2;
            dt2 = rci.dt;

            // Regula falsi
            if (dt1 * dt2 < 0.0f)
            {
                float tnext = 0.0f;
                // "we use the simplest possible approach by switching
                // to the bisection every 4th iteration:"
                if ((i & 3) == 0)
                {
                    tnext = 0.5f * (told + t);
                }
                else
                {
                    tnext = (dt2 * told - dt1 * t) / (dt2 - dt1);
                }
                told = t;
                t = tnext;
            }
            else
            {
                told = t;
                t += rci.dt;
            }

            if (t < 0.0f || t > 1.0f)
            {
                break;
            }
        }

        if (!result.hit)
        {
            tstart = 1.0f - tstart;
        }
        else
        {
            break;
        }
    }

    return result;
}

inline vec3 get_normal(
        hit_record<basic_ray<float>, primitive<unsigned>> const& hr,
        Curve const& curve
        )
{
    float t = hr.u;
    vec3 curve_pos = curve.f(t);
    return normalize(hr.isect_pos - curve_pos);
}

inline float area(Curve const& curve)
{
    VSNRAY_UNUSED(curve);

    // TODO: implement this to support curve lights!
    return -1.0f;
}

inline void split_primitive(aabb& L, aabb& R, float plane, int axis, Curve const& curve)
{
    VSNRAY_UNUSED(L);
    VSNRAY_UNUSED(R);
    VSNRAY_UNUSED(plane);
    VSNRAY_UNUSED(axis);
    VSNRAY_UNUSED(curve);

    // TODO: implement this to support SBVHs
}

//-------------------------------------------------------------------------------------------------
// I/O utility for camera lookat only - not fit for the general case!
//

std::istream& operator>>(std::istream& in, pinhole_camera& cam)
{
    vec3 eye;
    vec3 center;
    vec3 up;

    in >> eye >> std::ws >> center >> std::ws >> up >> std::ws;
    cam.look_at(eye, center, up);

    return in;
}

std::ostream& operator<<(std::ostream& out, pinhole_camera const& cam)
{
    out << cam.eye() << '\n';
    out << cam.center() << '\n';
    out << cam.up() << '\n';
    return out;
}

//-------------------------------------------------------------------------------------------------
// struct with state variables
//

struct renderer : viewer_type
{
    renderer()
        : viewer_type(512, 512, "Visionaray Phantom Ray-Hair Intersector Example")
        , host_sched(8)
    {
        using namespace support;

        // Add cmdline options
        add_cmdline_option( cl::makeOption<std::string&>(
            cl::Parser<>(),
            "filename",
            cl::Desc("Input file in wavefront obj format"),
            cl::Positional,
            cl::Optional,
            cl::init(this->filename)
            ) );

        add_cmdline_option( cl::makeOption<std::string&>(
            cl::Parser<>(),
            "camera",
            cl::Desc("Text file with camera parameters"),
            cl::ArgRequired,
            cl::init(this->initial_camera)
            ) );

        add_cmdline_option( cl::makeOption<unsigned&>(
            cl::Parser<>(),
            "spp",
            cl::Desc("Pixels per sample for path tracing"),
            cl::ArgRequired,
            cl::init(this->spp)
            ) );
    }

    void build_scene()
    {
        std::vector<Curve> actual_curves;

        std::shared_ptr<pbrt::Scene> scene;

        if (!filename.empty())
        {
            try
            {
                boost::filesystem::path p(filename);
                std::string ext = p.extension().string();

                std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower);

                std::cout << "Try loading pbrt file...\n";

                if (ext == ".pbf")
                {
                    scene = pbrt::Scene::loadFrom(filename);
                }
                else if (ext == ".pbrt")
                {
                    scene = pbrt::importPBRT(filename);
                }

                for (pbrt::Shape::SP shape : scene->world->shapes)
                {
                    if (pbrt::Curve::SP curve = std::dynamic_pointer_cast<pbrt::Curve>(shape))
                    {
                        if (curve->P.size() != 4)
                        {
                            continue;
                        }

                        vec3 w0(curve->P[0].x, curve->P[0].y, curve->P[0].z);
                        vec3 w1(curve->P[1].x, curve->P[1].y, curve->P[1].z);
                        vec3 w2(curve->P[2].x, curve->P[2].y, curve->P[2].z);
                        vec3 w3(curve->P[3].x, curve->P[3].y, curve->P[3].z);
                        // TODO: phantom _should_ also support two radii!
                        float r = (curve->width0 + curve->width1) * 0.5f;

                        actual_curves.push_back(make_curve(w0, w1, w2, w3, r));
                    }
                }
            }
            catch (std::runtime_error e)
            {
                std::cout << "Failed: " << e.what() << '\n';
                // ignore
            }
        }

        // Add some dummy data when file couldn't be loaded / file name was empty
        if (actual_curves.empty())
        {
            actual_curves.push_back(make_curve({0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.5f}, {2.0f, 0.5f, 0.0f}, {3.0f, 0.0f, 0.0f}, 0.3f));
            actual_curves.push_back(make_curve({0.0f, 0.0f, 0.0f}, {1.0f, 1.0f, 0.0f}, {2.0f, 2.0f, 0.0f}, {3.0f, 3.0f, 0.0f}, 0.08f));
            actual_curves.push_back(make_curve({0.0f, 0.0f, 0.0f}, {0.0f, 20.0f, 1.0f}, {0.0f, 0.0f, 2.0f}, {0.0f, 0.0f, 3.0f}, 0.04f));
            actual_curves.push_back(make_curve({0.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 1.0f}, {0.0f, 1.0f, 2.0f}, {0.0f, 0.0f, 3.0f}, 0.08f));
            actual_curves.push_back(make_curve({-1.0f, 2.0f, 0.0f}, {0.0f, -2.0f, 0.0f}, {1.0f, 4.0f, 0.0f}, {2.0f, -4.0f, 0.0f}, 0.02f));
            actual_curves.push_back(make_curve({-5.0f, 2.0f, 0.0f}, {0.0f, -0.5f, 1.0f}, {1.0f, 4.0f, 0.0f}, {2.0f, -4.0f, 8.0f}, 0.07f));
            actual_curves.push_back(make_curve({0.0f, 10.0f, 0.0f}, {0.0f, 11.0f, 11.0f}, {0.0f, -11.0f, 0.0f}, {0.0f, -10.0f, 0.0f}, 0.3f));
            actual_curves.push_back(make_curve({3.0f, 8.0f, 0.0f}, {0.0f, 15.0f, 11.0f}, {0.0f, -11.0f, 0.0f}, {7.0f, -10.0f, 0.0f}, 0.2f));
            actual_curves.push_back(make_curve({-10.0f, 20.0f, 0.0f}, {0.0f, -20.0f, 0.0f}, {1.0f, 4.0f, 0.0f}, {2.0f, -4.0f, 0.0f}, 0.1f));
            actual_curves.push_back(make_curve({0.0f, 20.0f, 0.0f}, {1.0f, 2.0f, 4.5f}, {2.0f, 3.5f, 0.0f}, {3.0f, 4.0f, 0.0f}, 0.3f));
        }

        for (auto const& curve : actual_curves)
        {
            auto p0 = split(curve);
            auto p00 = split(p0.first);
            auto p01 = split(p0.second);
            // auto p000 = split(p00.first);
            // auto p001 = split(p00.second);
            // auto p010 = split(p01.first);
            // auto p011 = split(p01.second);

            // 4x
            curves.push_back(p00.first);
            curves.push_back(p00.second);
            curves.push_back(p01.first);
            curves.push_back(p01.second);

            // 8x
            // curves.push_back(p000.first);
            // curves.push_back(p000.second);
            // curves.push_back(p001.first);
            // curves.push_back(p001.second);
            // curves.push_back(p010.first);
            // curves.push_back(p010.second);
            // curves.push_back(p011.first);
            // curves.push_back(p011.second);
        }

        for (unsigned i = 0; i < curves.size(); ++i)
        {
            curves[i].prim_id = i;
            curves[i].geom_id = 0;
        }

        std::cout << "Curves loaded:          " << actual_curves.size() << '\n';
        std::cout << "Curves after splitting: " << curves.size() << '\n';

        matte<float> m;
        m.cd() = from_rgb(0.78f, 0.70f, 0.55f);
        m.kd() = 1.0f;
        materials.emplace_back(m);

        binned_sah_builder builder;
        // TODO: implement get_split() for Curve to support spatial splits / SBVH!
        builder.enable_spatial_splits(false);

        std::cout << "Building BVH...\n";
        bvh = builder.build(index_bvh<Curve>{}, curves.data(), curves.size());
        std::cout << "Done!\n";

        bbox.invalidate();

        for (auto const& curve : curves)
        {
            bboxes.push_back(get_bounds(curve));
            bbox.insert(get_bounds(curve));
        }
    }

    thin_lens_camera                            cam;
    cpu_buffer_rt<PF_RGBA32F, PF_UNSPECIFIED>   host_rt;
    tiled_sched<basic_ray<float>>               host_sched;

    std::vector<Curve>                          curves;
    std::vector<aabb>                           bboxes;
    std::vector<matte<float>>                   materials;
    index_bvh<Curve>                            bvh;

    aabb                                        bbox;

    unsigned                                    frame_num       = 0;
    unsigned                                    spp             = 1;
    vec3                                        ambient         = vec3(1.0f, 1.0f, 1.0f);

    std::string                                 filename;
    std::string                                 initial_camera;

protected:

    void load_camera(std::string filename);
    void screenshot();
    void on_display();
    void on_key_press(visionaray::key_event const& event);
    void on_mouse_move(visionaray::mouse_event const& event);
    void on_resize(int w, int h);

};

//-------------------------------------------------------------------------------------------------
// Load camera from file, reset frame counter and clear frame
//

void renderer::load_camera(std::string filename)
{
    std::ifstream file(filename);
    if (file.good())
    {
        file >> cam;
        frame_num = 0;
        host_rt.clear_color_buffer();
        std::cout << "Load camera from file: " << filename << '\n';
    }
}

//-------------------------------------------------------------------------------------------------
// Take a screenshot
//

void renderer::screenshot()
{
    std::string screenshot_file_base = "screenshot";
#if VSNRAY_COMMON_HAVE_PNG
    static const std::string screenshot_file_suffix = ".png";
    image::save_option opt1;
#else
    static const std::string screenshot_file_suffix = ".pnm";
    image::save_option opt1({"binary", true});
#endif

    // Swizzle to RGB8 for compatibility with pnm image
    std::vector<vector<3, unorm<8>>> rgb(host_rt.width() * host_rt.height());
    swizzle(
        rgb.data(),
        PF_RGB8,
        host_rt.color(),
        PF_RGBA32F,
        host_rt.width() * host_rt.height(),
        TruncateAlpha
        );

    //if (rt.color_space() == host_device_rt::SRGB)
    {
        for (int y = 0; y < host_rt.height(); ++y)
        {
            for (int x = 0; x < host_rt.width(); ++x)
            {
                auto& color = rgb[y * host_rt.width() + x];
                color.x = powf(color.x, 1 / 2.2f);
                color.y = powf(color.y, 1 / 2.2f);
                color.z = powf(color.z, 1 / 2.2f);
            }
        }
    }

    // Flip so that origin is (top|left)
    std::vector<vector<3, unorm<8>>> flipped(host_rt.width() * host_rt.height());

    for (int y = 0; y < host_rt.height(); ++y)
    {
        for (int x = 0; x < host_rt.width(); ++x)
        {
            int yy = host_rt.height() - y - 1;
            flipped[yy * host_rt.width() + x] = rgb[y * host_rt.width() + x];
        }
    }

    image img(
        host_rt.width(),
        host_rt.height(),
        PF_RGB8,
        reinterpret_cast<uint8_t const*>(flipped.data())
        );

    int inc = 0;
    std::string inc_str = "";

    std::string filename = screenshot_file_base + inc_str + screenshot_file_suffix;

    while (boost::filesystem::exists(filename))
    {
        ++inc;
        inc_str = std::to_string(inc);

        while (inc_str.length() < 4)
        {
            inc_str = std::string("0") + inc_str;
        }

        inc_str = std::string("-") + inc_str;

        filename = screenshot_file_base + inc_str + screenshot_file_suffix;
    }

    if (img.save(filename, {opt1}))
    {
        std::cout << "Screenshot saved to file: " << filename << '\n';
    }
    else
    {
        std::cerr << "Error saving screenshot to file: " << filename << '\n';
    }
}

//-------------------------------------------------------------------------------------------------
// Display function
//

void renderer::on_display()
{
    // some setup

    //-----------------------------------------------------
    // Setup scheduler parameters.
    //

    // We use path tracing. We therefore use the
    // jittered_blend_type pixel sampler. That way
    // a rand generator is created, and noisy
    // images are blended later on.

    // Note how we set sfactor and dfactor based on
    // frame_num so that pixel blending works.

    // Note: for the jittered_blend_type pixel sampler to
    // work properly, you have to count frames yourself!
    // (check the occurrences of renderer::frame_num in
    // this file!)

    pixel_sampler::jittered_blend_type blend_params;
    blend_params.spp = spp;
    float alpha = 1.0f / ++frame_num;
    blend_params.sfactor = alpha;
    blend_params.dfactor = 1.0f - alpha;

    // Note: alternative samplers are:
    //  pixel_sampler::uniform_type
    // You can also leave this argument out, then you'll get
    // the default: pixel_sampler::uniform_type
    auto sparams = make_sched_params(
            blend_params,
            cam,          // the camera object (note: could also be two matrices!)
            host_rt       // render target, that's where we store the pixel result.
            );

    // Create bvh "refs" that we can pass to the
    // path tracing kernel
    using bvh_ref = index_bvh<Curve>::bvh_ref;
    aligned_vector<bvh_ref> primitives;
    primitives.push_back(bvh.ref());

    // Construct a parameter object that is
    // compatible with the builtin path tracing kernel.
    auto kparams = make_kernel_params(
            primitives.data(),
            primitives.data() + primitives.size(),
            materials.data(),
            4,      // bounces
            1e-5f,  // scene epsilon
            vec4(background_color(), 1.0f),
            vec4(ambient, 1.0f)
            );

    //-----------------------------------------------------
    // Naive path tracing with the builtin kernel.
    //

    // Instantiate the path tracing kernel, and
    // call it by executing the scheduler's
    // frame() function.
    pathtracing::kernel<decltype(kparams)> kernel;
    kernel.params = kparams;
    host_sched.frame(kernel, sparams);

    // Display the rendered image with OpenGL.

    glEnable(GL_FRAMEBUFFER_SRGB);
    glClearColor(0.0, 0.0, 0.0, 1.0);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    // You could also directly access host_rt::color()
    // or host_rt::depth() (this render target however
    // doesn't store a depth buffer).
    host_rt.display_color_buffer();
}

void renderer::on_key_press(visionaray::key_event const& event)
{
    static const std::string camera_file_base = "visionaray-camera";
    static const std::string camera_file_suffix = ".txt";

    switch (event.key())
    {
    case 'p':
        screenshot();
        break;

    case 'u':
        {
            int inc = 0;
            std::string inc_str = "";

            std::string filename = camera_file_base + inc_str + camera_file_suffix;

            while (boost::filesystem::exists(filename))
            {
                ++inc;
                inc_str = std::to_string(inc);

                while (inc_str.length() < 4)
                {
                    inc_str = std::string("0") + inc_str;
                }

                inc_str = std::string("-") + inc_str;

                filename = camera_file_base + inc_str + camera_file_suffix;
            }

            std::ofstream file(filename);
            if (file.good())
            {
                std::cout << "Storing camera to file: " << filename << '\n';
                file << cam;
            }
        }
        break;

    case 'v':
        {
            std::string filename = camera_file_base + camera_file_suffix;

            load_camera(filename);
        }
        break;

    default:
        break;
    }

    viewer_type::on_key_press(event);
}

//-------------------------------------------------------------------------------------------------
// mouse handling
//

void renderer::on_mouse_move(visionaray::mouse_event const& event)
{
    if (event.buttons() != mouse::NoButton)
    {
        frame_num = 0;
        host_rt.clear_color_buffer();
    }

    viewer_type::on_mouse_move(event);
}

//-------------------------------------------------------------------------------------------------
// resize event
//

void renderer::on_resize(int w, int h)
{
    frame_num = 0;
    host_rt.clear_color_buffer();

    cam.set_viewport(0, 0, w, h);
    float aspect = w / static_cast<float>(h);
    cam.perspective(45.0f * constants::degrees_to_radians<float>(), aspect, 0.001f, 1000.0f);
    host_rt.resize(w, h);

    viewer_type::on_resize(w, h);
}

//-------------------------------------------------------------------------------------------------
// Main function, performs initialization
//

int main(int argc, char** argv)
{
    renderer rend;

    try
    {
        rend.init(argc, argv);
    }
    catch (std::exception const& e)
    {
        std::cerr << e.what() << '\n';
        return EXIT_FAILURE;
    }

    rend.build_scene();

    float aspect = rend.width() / static_cast<float>(rend.height());

    rend.cam.perspective(45.0f * constants::degrees_to_radians<float>(), aspect, 0.001f, 1000.0f);
    rend.cam.set_lens_radius(0.002f);
    rend.cam.set_focal_distance(2.0f);

    // Load camera from file or set view-all
    std::ifstream file(rend.initial_camera);
    if (file.good())
    {
        file >> rend.cam;
    }
    else
    {
        rend.cam.view_all(rend.bbox);
    }

    rend.add_manipulator( std::make_shared<arcball_manipulator>(rend.cam, mouse::Left) );
    rend.add_manipulator( std::make_shared<pan_manipulator>(rend.cam, mouse::Middle) );
    // Additional "Alt + LMB" pan manipulator for setups w/o middle mouse button
    rend.add_manipulator( std::make_shared<pan_manipulator>(rend.cam, mouse::Left, keyboard::Alt) );
    rend.add_manipulator( std::make_shared<zoom_manipulator>(rend.cam, mouse::Right) );

    rend.event_loop();
}


/*
 * NormalVectorsFilter.cpp
 *
 *  Created on: May 05, 2015
 *      Author: Peter Fankhauser, Martin Wermelinger
 *   Institute: ETH Zurich, ANYbotics
 */

#include "grid_map_filters/NormalVectorsFilter.hpp"

#include <math.h>
#include <memory>
#include <stdexcept>

#include <tbb/task_scheduler_init.h>
#include <tbb/tbb.h>
#include <Eigen/Dense>

#include <grid_map_core/grid_map_core.hpp>

namespace grid_map {

NormalVectorsFilter::NormalVectorsFilter()
    : method_(Method::RasterSerial), estimationRadius_(0.0), parallelizationEnabled_(false), threadCount_(1), gridMapResolution_(0.02) {}

NormalVectorsFilter::~NormalVectorsFilter() = default;

bool NormalVectorsFilter::configure() {
  // Read which algorithm is chosen: area or raster.
  std::string algorithm;
  if (!FilterBase::getParam(std::string("algorithm"), algorithm)) {
    ROS_WARN("Could not find the parameter: `algorithm`. Setting to default value: 'area'.");
    // Default value.
    algorithm = "area";
  }

  // Read parameters related to area algorithm only if needed, otherwise when using raster method
  // on purpose it throws unwanted errors.
  if (algorithm != "raster") {
    // Read radius, if found, its value will be used for area method. If radius parameter is not found, raster method will be used.
    if (!FilterBase::getParam(std::string("radius"), estimationRadius_)) {
      ROS_WARN("Could not find the parameter: `radius`. Switching to raster method.");
      algorithm = "raster";
    }
    ROS_DEBUG("Normal vectors estimation radius = %f", estimationRadius_);
    // If radius not positive switch to raster method.
    if (estimationRadius_ <= 0) {
      ROS_WARN("Parameter `radius` is not positive. Switching to raster method.");
      algorithm = "raster";
    }
  }

  // Read parallelization_enabled to decide whether parallelization has to be used, if parameter is not found an error is thrown and
  // the false default value will be used.
  if (!FilterBase::getParam(std::string("parallelization_enabled"), parallelizationEnabled_)) {
    ROS_WARN("Could not find the parameter: `parallelization_enabled`. Setting to default value: 'false'.");
    parallelizationEnabled_ = false;
  }
  ROS_DEBUG("Parallelization_enabled = %d", parallelizationEnabled_);

  // Read thread_number to set the number of threads to be used if parallelization is enebled,
  // if parameter is not found an error is thrown and the default is to set it to automatic.
  if (!FilterBase::getParam(std::string("thread_number"), threadCount_)) {
    ROS_WARN("Could not find the parameter: `thread_number`. Setting to default value: 'automatic'.");
    threadCount_ = tbb::task_scheduler_init::automatic;
  }
  ROS_DEBUG("Thread_number = %d", threadCount_);

  // Set wanted method looking at algorithm and parallelization_enabled parameters.
  // parallelization_enabled is used to select whether to use parallelization or not.
  if (algorithm == "raster") {
    // If parallelizationEnabled_=true, use the parallel method, otherwise serial.
    if (parallelizationEnabled_) {
      method_ = Method::RasterParallel;
      ROS_DEBUG("Method RasterParallel");
    } else {
      method_ = Method::RasterSerial;
      ROS_DEBUG("Method RasterSerial");
    }
  } else {
    // If parallelizationEnabled_=true, use the parallel method, otherwise serial.
    if (parallelizationEnabled_) {
      method_ = Method::AreaParallel;
      ROS_DEBUG("Method AreaParallel");
    } else {
      method_ = Method::AreaSerial;
      ROS_DEBUG("Method AreaSerial");
    }
    ROS_DEBUG("estimationRadius_ = %f", estimationRadius_);
  }

  // Read normal_vector_positive_axis, to define normal vector positive direction.
  std::string normalVectorPositiveAxis;
  if (!FilterBase::getParam(std::string("normal_vector_positive_axis"), normalVectorPositiveAxis)) {
    ROS_ERROR("Normal vectors filter did not find parameter `normal_vector_positive_axis`.");
    return false;
  }
  if (normalVectorPositiveAxis == "z") {
    normalVectorPositiveAxis_ = Vector3::UnitZ();
  } else if (normalVectorPositiveAxis == "y") {
    normalVectorPositiveAxis_ = Vector3::UnitY();
  } else if (normalVectorPositiveAxis == "x") {
    normalVectorPositiveAxis_ = Vector3::UnitX();
  } else {
    ROS_ERROR("The normal vector positive axis '%s' is not valid.", normalVectorPositiveAxis.c_str());
    return false;
  }

  // Read input_layer, to define input grid map layer.
  if (!FilterBase::getParam(std::string("input_layer"), inputLayer_)) {
    ROS_ERROR("Normal vectors filter did not find parameter `input_layer`.");
    return false;
  }
  ROS_DEBUG("Normal vectors filter input layer is = %s.", inputLayer_.c_str());

  // Read output_layers_prefix, to define output grid map layers prefix.
  if (!FilterBase::getParam(std::string("output_layers_prefix"), outputLayersPrefix_)) {
    ROS_ERROR("Normal vectors filter did not find parameter `output_layers_prefix`.");
    return false;
  }
  ROS_DEBUG("Normal vectors filter output_layer = %s.", outputLayersPrefix_.c_str());

  // If everything has been set up correctly
  return true;
}

bool NormalVectorsFilter::update(const GridMap& mapIn, GridMap& mapOut) {
  std::vector<std::string> normalVectorsLayers;
  normalVectorsLayers.push_back(outputLayersPrefix_ + "x");
  normalVectorsLayers.push_back(outputLayersPrefix_ + "y");
  normalVectorsLayers.push_back(outputLayersPrefix_ + "z");

  mapOut = mapIn;
  for (const auto& layer : normalVectorsLayers) {
    mapOut.add(layer);
  }
  switch (method_) {
    case Method::AreaSerial:
      computeWithAreaSerial(mapOut, inputLayer_, outputLayersPrefix_);
      break;
    case Method::RasterSerial:
      computeWithRasterSerial(mapOut, inputLayer_, outputLayersPrefix_);
      break;
    case Method::AreaParallel:
      computeWithAreaParallel(mapOut, inputLayer_, outputLayersPrefix_);
      break;
    case Method::RasterParallel:
      computeWithRasterParallel(mapOut, inputLayer_, outputLayersPrefix_);
      break;
  }

  return true;
}

// SVD Area based methods.
void NormalVectorsFilter::computeWithAreaSerial(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix) {
  const double start = ros::Time::now().toSec();

  // For each cell in submap.
  for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
    // Check if this is an empty cell (hole in the map).
    if (map.isValid(*iterator, inputLayer)) {
      const Index index(*iterator);
      areaSingleNormalComputation(map, inputLayer, outputLayersPrefix, index);
    }
  }

  const double end = ros::Time::now().toSec();
  ROS_DEBUG_THROTTLE(2.0, "NORMAL COMPUTATION TIME = %f", (end - start));
}

void NormalVectorsFilter::computeWithAreaParallel(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix) {
  const double start = ros::Time::now().toSec();
  grid_map::Size gridMapSize = map.getSize();

  // Set number of thread to use for parallel programming.
  std::unique_ptr<tbb::task_scheduler_init> TBBInitPtr;
  if (threadCount_ != -1) {
    TBBInitPtr.reset(new tbb::task_scheduler_init(threadCount_));
  }

  // Parallelized iteration through the map.
  tbb::parallel_for(0, gridMapSize(0) * gridMapSize(1), [&](int range) {
    // Recover Cell index from range iterator.
    const Index index(range / gridMapSize(1), range % gridMapSize(1));
    if (map.isValid(index, inputLayer)) {
      areaSingleNormalComputation(map, inputLayer, outputLayersPrefix, index);
    }
  });

  const double end = ros::Time::now().toSec();
  ROS_DEBUG_THROTTLE(2.0, "NORMAL COMPUTATION TIME = %f", (end - start));
}

void NormalVectorsFilter::areaSingleNormalComputation(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix,
                                                      const grid_map::Index& index) {
  // Requested position (center) of circle in map.
  Position center;
  map.getPosition(index, center);

  // Prepare data computation. Check if area is bigger than cell.
  const double minAllowedEstimationRadius = 0.5 * map.getResolution();
  if (estimationRadius_ <= minAllowedEstimationRadius) {
    ROS_WARN("Estimation radius is smaller than allowed by the map resolution (%f < %f)", estimationRadius_, minAllowedEstimationRadius);
  }

  // Gather surrounding data.
  size_t nPoints = 0;
  Position3 sum = Position3::Zero();
  Eigen::Matrix3d sumSquared = Eigen::Matrix3d::Zero();
  for (CircleIterator circleIterator(map, center, estimationRadius_); !circleIterator.isPastEnd(); ++circleIterator) {
    Position3 point;
    if (!map.getPosition3(inputLayer, *circleIterator, point)) {
      continue;
    }
    nPoints++;
    sum += point;
    sumSquared.noalias() += point * point.transpose();
  }

  Vector3 unitaryNormalVector = Vector3::Zero();
  if (nPoints < 3) {
    ROS_DEBUG("Not enough points to establish normal direction (nPoints = %i)", static_cast<int>(nPoints));
    unitaryNormalVector = Vector3::UnitZ();
  } else {
    const Position3 mean = sum / nPoints;
    const Eigen::Matrix3d covarianceMatrix = sumSquared / nPoints - mean * mean.transpose();

    // Compute Eigenvectors.
    // Eigenvalues are ordered small to large.
    // Worst case bound for zero eigenvalue from : https://eigen.tuxfamily.org/dox/classEigen_1_1SelfAdjointEigenSolver.html
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver;
    solver.computeDirect(covarianceMatrix, Eigen::DecompositionOptions::ComputeEigenvectors);
    if (solver.eigenvalues()(1) > 1e-8) {
      unitaryNormalVector = solver.eigenvectors().col(0);
    } else {  // If second eigenvalue is zero, the normal is not defined.
      ROS_DEBUG("Covariance matrix needed for eigen decomposition is degenerated.");
      ROS_DEBUG("Expected cause: data is on a straight line (nPoints = %i)", static_cast<int>(nPoints));
      unitaryNormalVector = Vector3::UnitZ();
    }
  }

  // Check direction of the normal vector and flip the sign towards the user defined direction.
  if (unitaryNormalVector.dot(normalVectorPositiveAxis_) < 0.0) {
    unitaryNormalVector = -unitaryNormalVector;
  }

  map.at(outputLayersPrefix + "x", index) = unitaryNormalVector.x();
  map.at(outputLayersPrefix + "y", index) = unitaryNormalVector.y();
  map.at(outputLayersPrefix + "z", index) = unitaryNormalVector.z();
}
// Raster based methods.
void NormalVectorsFilter::computeWithRasterSerial(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix) {
  // Inspiration for algorithm: http://www.flipcode.com/archives/Calculating_Vertex_Normals_for_Height_Maps.shtml
  const double start = ros::Time::now().toSec();

  const grid_map::Size gridMapSize = map.getSize();
  gridMapResolution_ = map.getResolution();
  // Faster access to grid map values.
  const grid_map::Matrix dataMap = map[inputLayer];
  // Height and width of submap. Submap is Map without the outermost line of cells, no need to check if index is inside.
  const Index submapStartIndex(1, 1);
  const Index submapBufferSize(gridMapSize(0) - 2, gridMapSize(1) - 2);

  // For each cell in submap.
  for (SubmapIterator iterator(map, submapStartIndex, submapBufferSize); !iterator.isPastEnd(); ++iterator) {
    const Index index(*iterator);
    rasterSingleNormalComputation(map, outputLayersPrefix, dataMap, index);
  }

  const double end = ros::Time::now().toSec();
  ROS_DEBUG_THROTTLE(2.0, "NORMAL COMPUTATION TIME = %f", (end - start));
}

void NormalVectorsFilter::computeWithRasterParallel(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix) {
  const double start = ros::Time::now().toSec();

  const grid_map::Size gridMapSize = map.getSize();
  gridMapResolution_ = map.getResolution();
  // Faster access to grid map values if copy grid map layer into local matrix.
  const grid_map::Matrix dataMap = map[inputLayer];
  // Height and width of submap. Submap is Map without the outermost line of cells, no need to check if index is inside.
  const Index submapStartIndex(1, 1);
  const Index submapBufferSize(gridMapSize(0) - 2, gridMapSize(1) - 2);
  if (submapBufferSize(1) != 0) {
    // Set number of thread to use for parallel programming
    std::unique_ptr<tbb::task_scheduler_init> TBBInitPtr;
    if (threadCount_ != -1) {
      TBBInitPtr.reset(new tbb::task_scheduler_init(threadCount_));
    }
    // Parallelized iteration through the map.
    tbb::parallel_for(0, submapBufferSize(0) * submapBufferSize(1), [&](int range) {
      const Index index(range / submapBufferSize(1) + submapStartIndex(0), range % submapBufferSize(1) + submapStartIndex(1));
      rasterSingleNormalComputation(map, outputLayersPrefix, dataMap, index);
    });
  } else {
    ROS_ERROR("Grid map size is too small for normal raster computation");
  }

  const double end = ros::Time::now().toSec();
  ROS_DEBUG_THROTTLE(2.0, "NORMAL COMPUTATION TIME = %f", (end - start));
}

void NormalVectorsFilter::rasterSingleNormalComputation(GridMap& map, const std::string& outputLayersPrefix,
                                                        const grid_map::Matrix& dataMap, const grid_map::Index& index) {
  // Inspiration for algorithm:
  // http://www.flipcode.com/archives/Calculating_Vertex_Normals_for_Height_Maps.shtml
  const double centralCell = dataMap(index(0), index(1));
  double topCell = dataMap(index(0) - 1, index(1));
  double rightCell = dataMap(index(0), index(1) + 1);
  double bottomCell = dataMap(index(0) + 1, index(1));
  double leftCell = dataMap(index(0), index(1) - 1);

  // Neighboring cells configuration checked in X and Y direction independently.
  // Gridmap frame is defined rotated 90 degrees anticlockwise compared to direction of matrix if we take
  // rows as Y coordinate and columns as X coordinate.
  // In Y direction cell numbered as follows: left 0, center 1, right 2.
  // In X direction cell numbered as follows: top 0, center 1, bottom 2.
  // To find configuration we are in, multiply value of map.isValid of cell in question, True or False,
  // by 2^(number of the cell).
  // Each configuration will have a different number associated with it and then use a switch.

  const int configurationDirX = 1 * static_cast<int>(std::isfinite(topCell)) + 2 * static_cast<int>(std::isfinite(centralCell)) +
                                4 * static_cast<int>(std::isfinite(bottomCell));
  const int configurationDirY = 1 * static_cast<int>(std::isfinite(leftCell)) + 2 * static_cast<int>(std::isfinite(centralCell)) +
                                4 * static_cast<int>(std::isfinite(rightCell));

  // If outer cell height value is missing use the central value, however the formula for the normal calculation
  // has to take into account that the distance of the cells used for normal calculation is different.
  bool validConfiguration = true;
  double distanceX{NAN};
  switch (configurationDirX) {
    case 7:                                // All 3 cell height values are valid.
      distanceX = 2 * gridMapResolution_;  // Top and bottom cell centers are 2 cell resolution distant.
      break;
    case 6:  // Top cell height value not valid.
      topCell = centralCell;
      distanceX = gridMapResolution_;
      break;
    case 5:  // Central cell height value not valid. Not a problem.
      distanceX = 2 * gridMapResolution_;
      break;
    case 3:  // Bottom cell height value not valid.
      bottomCell = centralCell;
      distanceX = gridMapResolution_;
      break;

    default:  // More than 1 cell height values are not valid, normal vector will not be calculated in this location.
      validConfiguration = false;
  }

  double distanceY{NAN};
  switch (configurationDirY) {
    case 7:                                // All 3 cell height values are valid.
      distanceY = 2 * gridMapResolution_;  // Left and right cell centers are 2 call resolution distant.
      break;
    case 6:  // Left cell height value not valid.
      leftCell = centralCell;
      distanceY = gridMapResolution_;
      break;
    case 5:  // Central cell height value not valid. Not a problem.
      distanceY = 2 * gridMapResolution_;
      break;
    case 3:  // Right cell height value not valid.
      rightCell = centralCell;
      distanceY = gridMapResolution_;
      break;

    default:  // More than 1 cell height values are not valid, normal vector will not be calculated in this location.
      validConfiguration = false;
  }

  if (validConfiguration) {
    // Normal vector initialization
    Vector3 normalVector = Vector3::Zero();
    // X DIRECTION
    normalVector(0) = (bottomCell - topCell) / distanceX;
    // Y DIRECTION
    normalVector(1) = (rightCell - leftCell) / distanceY;
    // Z DIRECTION
    normalVector(2) = +1;

    normalVector.normalize();

    // Check direction of the normal vector and flip the sign towards the user defined direction.
    if (normalVector.dot(normalVectorPositiveAxis_) < 0.0) {
      normalVector = -normalVector;
    }

    map.at(outputLayersPrefix + "x", index) = normalVector.x();
    map.at(outputLayersPrefix + "y", index) = normalVector.y();
    map.at(outputLayersPrefix + "z", index) = normalVector.z();
  }
}

}  // namespace grid_map


/*
Copyright (c) 2018 Inverse Palindrome
Apophis - WanderComponent.cpp
InversePalindrome.com
*/

#include "WanderComponent.hpp"

#include <imgui.h>

#include <cocos/base/ccRandom.h>

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

WanderComponent::WanderComponent() :
    wanderDistance(5.f),
    wanderRadius(1.f),
    wanderRate(1.f),
    wanderAngle(cocos2d::RandomHelper::random_real(0.f, 2.f * boost::math::constants::pi<float>()))
{
}

void WanderComponent::load(const pugi::xml_node& componentNode)
{
    if (const auto wanderDistanceAttribute = componentNode.attribute("distance"))
    {
        setWanderDistance(wanderDistanceAttribute.as_float());
    }
    if (const auto wanderRadiusAttribute = componentNode.attribute("radius"))
    {
        setWanderRadius(wanderRadiusAttribute.as_float());
    }
    if (const auto wanderRateAttribute = componentNode.attribute("rate"))
    {
        setWanderRate(wanderRateAttribute.as_float());
    }
}

void WanderComponent::save(pugi::xml_node& componentNode) const
{
    componentNode.set_name("Wander");

    componentNode.append_attribute("distance") = getWanderDistance();
    componentNode.append_attribute("radius") = getWanderRadius();
    componentNode.append_attribute("rate") = getWanderRate();
}

void WanderComponent::display()
{
    if (ImGui::TreeNode("Wander"))
    {
        ImGui::InputFloat("Distance", &wanderDistance);
        ImGui::InputFloat("Radius", &wanderRadius);
        ImGui::InputFloat("Rate", &wanderRate);

        ImGui::TreePop();
    }
}

float WanderComponent::getWanderDistance() const
{
    return wanderDistance;
}

void WanderComponent::setWanderDistance(float wanderDistance)
{
    this->wanderDistance = wanderDistance;
}

float WanderComponent::getWanderRadius() const
{
    return wanderRadius;
}

void WanderComponent::setWanderRadius(float wanderRadius)
{
    this->wanderRadius = wanderRadius;
}

float WanderComponent::getWanderRate() const
{
    return wanderRate;
}

void WanderComponent::setWanderRate(float wanderRate)
{
    this->wanderRate = wanderRate;
}

float& WanderComponent::getWanderAngle()
{
    return wanderAngle;
}


#include <boost/functional/hash.hpp>
#include <boost/math/distributions.hpp>
#include <math.h>

#include "hashgrid.h"

#include <QDebug>

HashGrid::HashGrid(float _cellSize)
{
    qDebug("HashGrid Particle tmpl ctor 2");
//    auto a = boost::math::iround<float>(14.494);

}

void HashGrid::clear()
{
    m_buckets.clear();
}

void HashGrid::setGridSize(float _size)
{
    cellSize = _size;
}

float HashGrid::getGridSize()
{
    return cellSize;
}

bool HashGrid::insert(size_t _hashV, ParticlePtr _p)
{

    bool CellExists = false;
    if(cellExists(_hashV))
    {
        CellExists = true;
    }
    m_buckets[_hashV].push_back(_p);
    return CellExists;

}

bool HashGrid::cellExists(size_t _hashV)
{
    bool CellExists = false;
    std::unordered_map< size_t , std::list< ParticlePtr >>::const_iterator itr = m_buckets.find(_hashV);
    if ( itr == m_buckets.end())
    {
        CellExists = false;
    }
    else
    {
        CellExists = true;
    }
    return CellExists;
}

size_t HashGrid::hashFunction(int3 _cell)
{
        // seed prime number
        size_t hash = 85523717;
        boost::hash_combine(hash, _cell.i);
        boost::hash_combine(hash, _cell.j);
        boost::hash_combine(hash, _cell.k);
        return hash;
}

size_t HashGrid::hashFunciton(float _i, float _j, float _k)
{
    size_t hash = 85523717;
    boost::hash_combine(hash, _i);
    boost::hash_combine(hash, _j);
    boost::hash_combine(hash, _k);
    return hash;
}

int3 HashGrid::pointToCell(float _x, float _y, float _z)
{
    int3 cell;
    // rounding to full integer - coversion
    // optionally use: auto a = boost::math::iround<float>(_x);
//    cell.i = int(_x * (1 / cellSize));
//    cell.j = int(_y * (1 / cellSize));
//    cell.k = int(_z * (1 / cellSize));
    cell.i = int(_x * ( cellSize));
    cell.j = int(_y * ( cellSize));
    cell.k = int(_z * ( cellSize));
//    cell.i = int((_x + 0.1) * (1 / cellSize));
//    cell.j = int((_y + 0.1) * (1 / cellSize));
//    cell.k = int((_z + 0.1) * (1 / cellSize));
//    mlog<<cell.i<<" , "<<cell.j<<" , "<<cell.k<<" cellSize: "<<cellSize<<" - "<<_x<<" , "<<_y<<" , "<<_z;
    return cell;
}

std::list<ParticlePtr> HashGrid:: query(size_t _hash)
{
    std::list<ParticlePtr> cellList;
//    if(!cellExists(_hash))
//    {
//        return cellList;
//    }
//    else
    {
//        auto test = m_buckets[_hash];
        return m_buckets[_hash];
    }
}

std::list<ParticlePtr> HashGrid::cellNeighbours(int3 _cell)
{
    std::list<ParticlePtr> neighbourParticles_empty;

    std::list<ParticlePtr> neighbourParticles;
//    std::list<int3> neighbourCells;
//    size_t middleHash = hashFunction(_cell);

    int level = 1;
//    int length = level + 2;
//    int size = std::pow(length, 3);

    for(int y = -1 ; y <= level ; y++)
    {
        for(int x = -1 ; x <= level ; x++)
        {
            for(int z = -1 ; z <= level ; z++)
            {
                int3 nCell;
                nCell.i = _cell.i + x;
                nCell.j = _cell.j + y;
                nCell.k = _cell.k + z;

//                neighbourCells.push_back(nCell);

                // query for std::list<ParticlePtr>
                // and if possible merge
                size_t hash = hashFunction(nCell);
//                if(hash == middleHash) continue;

                if(cellExists(hash))
                {
//                    qDebug()<<"Found a hash: "<<x<<y<<z;
//                    std::list<ParticlePtr> currentCellList = query(hash);
//                    std::list<ParticlePtr> currentCellList = m_buckets[hash];
                    std::unordered_map< size_t , std::list< ParticlePtr >>::iterator test = m_buckets.find(hash);
//                    tmpList = m_buckets.find(hash);
                    for(auto n : test->second)
                    {
                        float p = n->position().x();
//                        neighbourParticles_empty.push_back(n);
                    }
//                    std::list<ParticlePtr>::const_iterator itr = neighbourParticles.end();
//                    std::list<ParticlePtr>::const_iterator itrNCellStart = currentCellList.begin();
//                    std::list<ParticlePtr>::const_iterator itrNCellEnd = currentCellList.end();
//                    neighbourParticles.insert(itr, itrNCellStart, itrNCellEnd);
                };
//                qDebug()<<"N: "<<n;
            }
        }
    }

//    qDebug()<<&neighbourParticles;
//    return neighbourParticles;
    return neighbourParticles_empty;
}

//int HashGrid::cellNeighboursFunc(int3 _cell, void *func)
//{
//    return 1;
//    std::list<ParticlePtr> neighbourParticles_empty;

//    std::list<ParticlePtr> neighbourParticles;
////    std::list<int3> neighbourCells;
////    size_t middleHash = hashFunction(_cell);

//    int level = 1;
////    int length = level + 2;
////    int size = std::pow(length, 3);
//    int n = 0;

//    for(int y = -1 ; y <= level ; y++)
//    {
//        for(int x = -1 ; x <= level ; x++)
//        {
//            for(int z = -1 ; z <= level ; z++)
//            {
//                int3 nCell;
//                nCell.i = _cell.i + x;
//                nCell.j = _cell.j + y;
//                nCell.k = _cell.k + z;

////                neighbourCells.push_back(nCell);

//                // query for std::list<ParticlePtr>
//                // and if possible merge
//                size_t hash = hashFunction(nCell);
////                if(hash == middleHash) continue;

//                if(cellExists(hash))
//                {
////                    qDebug()<<"Found a hash: "<<x<<y<<z;
////                    std::list<ParticlePtr> currentCellList = query(hash);
////                    std::list<ParticlePtr> currentCellList = m_buckets[hash];
//                    std::unordered_map< size_t , std::list< ParticlePtr >>::iterator test = m_buckets.find(hash);
////                    tmpList = m_buckets.find(hash);
//                    for(auto n : test->second)
//                    {
//                        float p = n->position().x();
//                    }
////                    std::list<ParticlePtr>::const_iterator itr = neighbourParticles.end();
////                    std::list<ParticlePtr>::const_iterator itrNCellStart = currentCellList.begin();
////                    std::list<ParticlePtr>::const_iterator itrNCellEnd = currentCellList.end();
////                    neighbourParticles.insert(itr, itrNCellStart, itrNCellEnd);
//                };
////                qDebug()<<"N: "<<n;
//                n++;
//            }
//        }
//    }

////    qDebug()<<&neighbourParticles;
////    return neighbourParticles;
//    return neighbourParticles_empty;
//}


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#include "ompl/control/spaces/RealVectorControlSpace.h"
#include "ompl/util/Exception.h"
#include <boost/lexical_cast.hpp>
#include <cstring>
#include <limits>

void ompl::control::RealVectorControlUniformSampler::sample(Control *control)
{
    const unsigned int dim = space_->getDimension();
    const base::RealVectorBounds &bounds = static_cast<const RealVectorControlSpace*>(space_)->getBounds();

    RealVectorControlSpace::ControlType *rcontrol = static_cast<RealVectorControlSpace::ControlType*>(control);
    for (unsigned int i = 0 ; i < dim ; ++i)
        rcontrol->values[i] = rng_.uniformReal(bounds.low[i], bounds.high[i]);
}

void ompl::control::RealVectorControlSpace::setup()
{
    ControlSpace::setup();
    bounds_.check();
}

void ompl::control::RealVectorControlSpace::setBounds(const base::RealVectorBounds &bounds)
{
    bounds.check();
    if (bounds.low.size() != dimension_)
        throw Exception("Bounds do not match dimension of control space: expected dimension " +
                        boost::lexical_cast<std::string>(dimension_) + " but got dimension " +
                        boost::lexical_cast<std::string>(bounds.low.size()));
    bounds_ = bounds;
}

unsigned int ompl::control::RealVectorControlSpace::getDimension() const
{
    return dimension_;
}

void ompl::control::RealVectorControlSpace::copyControl(Control *destination, const Control *source) const
{
    memcpy(static_cast<ControlType*>(destination)->values,
           static_cast<const ControlType*>(source)->values, controlBytes_);
}

bool ompl::control::RealVectorControlSpace::equalControls(const Control *control1, const Control *control2) const
{
    const double *s1 = static_cast<const ControlType*>(control1)->values;
    const double *s2 = static_cast<const ControlType*>(control2)->values;
    for (unsigned int i = 0 ; i < dimension_ ; ++i)
    {
        double diff = (*s1++) - (*s2++);
        if (fabs(diff) > std::numeric_limits<double>::epsilon() * 2.0)
            return false;
    }
    return true;
}

ompl::control::ControlSamplerPtr ompl::control::RealVectorControlSpace::allocDefaultControlSampler() const
{
    return ControlSamplerPtr(new RealVectorControlUniformSampler(this));
}

ompl::control::Control* ompl::control::RealVectorControlSpace::allocControl() const
{
    ControlType *rcontrol = new ControlType();
    rcontrol->values = new double[dimension_];
    return rcontrol;
}

void ompl::control::RealVectorControlSpace::freeControl(Control *control) const
{
    ControlType *rcontrol = static_cast<ControlType*>(control);
    delete[] rcontrol->values;
    delete rcontrol;
}

void ompl::control::RealVectorControlSpace::nullControl(Control *control) const
{
    ControlType *rcontrol = static_cast<ControlType*>(control);
    for (unsigned int i = 0 ; i < dimension_ ; ++i)
    {
        if (bounds_.low[i] <= 0.0 && bounds_.high[i] >= 0.0)
            rcontrol->values[i] = 0.0;
        else
            rcontrol->values[i] = bounds_.low[i];
    }
}

double* ompl::control::RealVectorControlSpace::getValueAddressAtIndex(Control *control, const unsigned int index) const
{
    return index < dimension_ ? static_cast<ControlType*>(control)->values + index : NULL;
}

void ompl::control::RealVectorControlSpace::printControl(const Control *control, std::ostream &out) const
{
    out << "RealVectorControl [";
    if (control)
    {
        const ControlType *rcontrol = static_cast<const ControlType*>(control);
        for (unsigned int i = 0 ; i < dimension_ ; ++i)
        {
            out << rcontrol->values[i];
            if (i + 1 < dimension_)
                out << ' ';
        }
    }
    else
        out << "NULL";
    out << ']' << std::endl;
}

void ompl::control::RealVectorControlSpace::printSettings(std::ostream &out) const
{
    out << "Real vector control space '" << getName() << "' with bounds: " << std::endl;
    out << "  - min: ";
    for (unsigned int i = 0 ; i < dimension_ ; ++i)
        out << bounds_.low[i] << " ";
    out << std::endl;
    out << "  - max: ";
    for (unsigned int i = 0 ; i < dimension_ ; ++i)
        out << bounds_.high[i] << " ";
    out << std::endl;
}

unsigned int ompl::control::RealVectorControlSpace::getSerializationLength() const
{
    return controlBytes_;
}

void ompl::control::RealVectorControlSpace::serialize(void *serialization, const Control *ctrl) const
{
    memcpy(serialization, ctrl->as<ControlType>()->values, controlBytes_);
}

void ompl::control::RealVectorControlSpace::deserialize(Control *ctrl, const void *serialization) const
{
    memcpy(ctrl->as<ControlType>()->values, serialization, controlBytes_);
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <string>
#include <vector>
#include <complex>
#include <boost/timer.hpp>

#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/morton_dense.hpp>
#include <boost/numeric/mtl/operation/print_matrix.hpp>
#include <boost/numeric/mtl/operation/matrix_mult.hpp>
#include <boost/numeric/mtl/matrix/hessian_setup.hpp>
#include <boost/numeric/mtl/operation/assign_mode.hpp>
#include <boost/numeric/mtl/recursion/predefined_masks.hpp>
#include <boost/numeric/mtl/utility/papi.hpp>

using namespace mtl;
using namespace mtl::recursion;
using namespace std;

// Maximum time for a single measurement
// is 20 min
const double max_time= 900;

typedef assign::plus_sum                            ama_t;

typedef recursion::bound_test_static<32>                    test32_t;
typedef recursion::bound_test_static<64>                    test64_t;

typedef gen_dense_mat_mat_mult_t<assign::plus_sum>  base_mult_t;
typedef gen_recursive_dense_mat_mat_mult_t<base_mult_t>     rec_mult_t;

typedef gen_tiling_22_dense_mat_mat_mult_t<assign::plus_sum>  tiling_22_base_mult_t;
typedef gen_tiling_44_dense_mat_mat_mult_t<assign::plus_sum>  tiling_44_base_mult_t;

// ugly short cuts
typedef dense2D<double>                                       dr_t;
typedef dense2D<double, mat::parameters<col_major> >        dc_t;

utility::papi_t papi;
int l1i= papi.add_event("PAPI_L1_DCM");
int l2i= papi.add_event("PAPI_L2_DCM");
int tlbi= papi.add_event("PAPI_TLB_DM");

#ifdef MTL_USE_BLAS
extern "C" {
void dgemm_(const char* transa, const char* transb,
	    const int* m, const int* n, const int* k,
	    const double* alpha,  const double *da,  const int* lda,
	    const double *db, const int* ldb, const double* dbeta,
	    double *dc, const int* ldc);
}
#endif

struct dgemm_t
{
    void operator()(const dc_t& a, const dc_t& b, dc_t& c)
    {
#ifdef MTL_USE_BLAS
	int size= a.num_rows();
	double alpha= 1.0, beta= 0.0;
	dgemm_("N", "N", &size, &size, &size, &alpha,
	       const_cast<double*>(&a[0][0]), &size, const_cast<double*>(&b[0][0]),
	       &size, &beta, &c[0][0], &size);
#endif
    }
};

struct dgemm_add_t
{
    void operator()(const dc_t& a, const dc_t& b, dc_t& c)
    {
	int size= a.num_rows();
	double alpha= 1.0, beta= 1.0;
	dgemm_("N", "N", &size, &size, &size, &alpha,
	       const_cast<double*>(&a[0][0]), &size, const_cast<double*>(&b[0][0]),
	       &size, &beta, &c[0][0], &size);

    }
};

void print_time_and_mflops(double time, double size)
{
    // time and MFlops of single measure
    std::cout << time << ", " << 2.0 * size * size * size / time / 1e6f << ", ";
}

// Matrices are only placeholder to provide the type
template <typename MatrixA, typename MatrixB, typename MatrixC, typename Mult>
void single_measure(MatrixA&, MatrixB&, MatrixC&, Mult mult, unsigned size, std::vector<int>& enabled, int i)
{
    MatrixA a(size, size);
    MatrixB b(size, size);
    MatrixC c(size, size);
    hessian_setup(a, 1.0);
    hessian_setup(b, 1.0);

    if (enabled[i]) {
	int reps= 0;
	boost::timer start;
	papi.reset();
	for (; start.elapsed() < 5; reps++)
	    mult(a, b, c);
	papi.read();
	double time= start.elapsed() / double(reps);

	print_time_and_mflops(time, a.num_rows());
	std::cout << papi[l1i]/reps << ", " << papi[l2i]/reps << ", " << papi[tlbi]/reps << ", ";
	if (time > max_time)
	    enabled[i]= 0;
    } else
	std::cout << ", , , , , ";
}

// The matrices in the following functions are only place holders, the real matrices are used in single_measure
void measure_morton_order(unsigned size, std::vector<int>& enabled)
{
    morton_dense<double,  morton_mask>             mda(4, 4), mdb(4, 4), mdc(4, 4);
    morton_dense<double,  morton_z_mask>           mzda(4, 4), mzdb(4, 4), mzdc(4, 4);
    dc_t                                           dc(4, 4);

    std::cout << size << ", ";
    rec_mult_t  mult;
    single_measure(mda, mdb, mdc, mult, size, enabled, 0);
    single_measure(mzda, mzdb, mzdc, mult, size, enabled, 1);
    single_measure(mda, mzdb, mdc, mult, size, enabled, 2);
    single_measure(dc, dc, dc, dgemm_t(), size, enabled, 3);

    std::cout << "0\n"; // to not finish with comma
    std::cout.flush();
}

void measure_cast(unsigned size, std::vector<int>& enabled)
{
    morton_dense<double,  morton_mask>             mda(4, 4), mdb(4, 4), mdc(4, 4);
    morton_dense<double,  doppled_32_row_mask>     d32ra(4, 4), d32rb(4, 4), d32rc(4, 4);
    morton_dense<double,  doppled_64_row_mask>     d64ra(4, 4), d64rb(4, 4), d64rc(4, 4);
    morton_dense<double,  doppled_64_col_mask>     d64ca(4, 4), d64cb(4, 4), d64cc(4, 4);

    rec_mult_t  mult;
    std::cout << size << ", ";
    single_measure(mda, mdb, mdc, mult, size, enabled, 0);
    single_measure(d32ra, d32rb, d32rc, mult, size, enabled, 1);
    single_measure(d64ra, d64rb, d64rc, mult, size, enabled, 2);
    single_measure(d64ca, d64cb, d64cc, mult, size, enabled, 3);

    dc_t                                           dc(4, 4);
    single_measure(dc, dc, dc, dgemm_t(), size, enabled, 4);

    std::cout << "0\n";  std::cout.flush();
}

void measure_with_unroll(unsigned size, std::vector<int>& enabled)
{
    morton_dense<double,  doppled_32_row_mask>     d32r(4, 4);
    morton_dense<double,  doppled_32_col_mask>     d32c(4, 4);

    std::cout << size << ", ";

    gen_recursive_dense_mat_mat_mult_t<base_mult_t, test32_t>           mult;
    gen_recursive_dense_mat_mat_mult_t<tiling_22_base_mult_t, test32_t> mult_22;
    gen_recursive_dense_mat_mat_mult_t<tiling_44_base_mult_t, test32_t> mult_44;

    single_measure(d32r, d32r, d32r, mult, size, enabled, 0);
    single_measure(d32r, d32r, d32r, mult_22, size, enabled, 1);
    single_measure(d32r, d32r, d32r, mult_44, size, enabled, 2);

    single_measure(d32c, d32c, d32c, mult, size, enabled, 3);
    single_measure(d32c, d32c, d32c, mult_22, size, enabled, 4);
    single_measure(d32c, d32c, d32c, mult_44, size, enabled, 5);
    dc_t                                           dc(4, 4);
    single_measure(dc, dc, dc, dgemm_t(), size, enabled, 6);

    std::cout << "0\n";  std::cout.flush();
}

void measure_base_size(unsigned size, std::vector<int>& enabled)
{
    morton_dense<double,  doppled_16_row_mask>     d16r(4, 4);
    morton_dense<double,  doppled_32_row_mask>     d32r(4, 4);
    morton_dense<double,  doppled_64_row_mask>     d64r(4, 4);
    morton_dense<double,  doppled_128_col_mask>    d128r(4, 4);

    std::cout << size << ", ";

    gen_recursive_dense_mat_mat_mult_t<tiling_22_base_mult_t, recursion::bound_test_static<16> > mult16;
    single_measure(d16r, d16r, d16r, mult16, size, enabled, 0);

    gen_recursive_dense_mat_mat_mult_t<tiling_22_base_mult_t, recursion::bound_test_static<32> > mult32;
    single_measure(d32r, d32r, d32r, mult32, size, enabled, 1);

    gen_recursive_dense_mat_mat_mult_t<tiling_22_base_mult_t, recursion::bound_test_static<64> > mult64;
    single_measure(d64r, d64r, d64r, mult64, size, enabled, 2);

    gen_recursive_dense_mat_mat_mult_t<tiling_22_base_mult_t, recursion::bound_test_static<128> > mult128;
    single_measure(d128r, d128r, d128r, mult128, size, enabled, 3);

    dc_t                                           dc(4, 4);
    single_measure(dc, dc, dc, dgemm_t(), size, enabled, 4);

    std::cout << "0\n";  std::cout.flush();
}

template <typename Matrix, typename MatrixB>
void measure_unrolling(unsigned size, std::vector<int>& enabled, Matrix& matrix, MatrixB& matrixb)
{
    std::cout << size << ", ";

    gen_recursive_dense_mat_mat_mult_t<base_mult_t>           mult;
    gen_recursive_dense_mat_mat_mult_t<tiling_22_base_mult_t> mult_22;
    gen_recursive_dense_mat_mat_mult_t<tiling_44_base_mult_t> mult_44;

    typedef gen_tiling_dense_mat_mat_mult_t<2, 2, ama_t>  tiling_m22_base_mult_t;
    gen_recursive_dense_mat_mat_mult_t<tiling_m22_base_mult_t> mult_m22;

    typedef gen_tiling_dense_mat_mat_mult_t<2, 4, ama_t>  tiling_m24_base_mult_t;
    gen_recursive_dense_mat_mat_mult_t<tiling_m24_base_mult_t> mult_m24;

    typedef gen_tiling_dense_mat_mat_mult_t<4, 2, ama_t>  tiling_m42_base_mult_t;
    gen_recursive_dense_mat_mat_mult_t<tiling_m42_base_mult_t> mult_m42;

    typedef gen_tiling_dense_mat_mat_mult_t<3, 5, ama_t>  tiling_m35_base_mult_t;
    gen_recursive_dense_mat_mat_mult_t<tiling_m35_base_mult_t> mult_m35;

    typedef gen_tiling_dense_mat_mat_mult_t<4, 4, ama_t>  tiling_m44_base_mult_t;
    gen_recursive_dense_mat_mat_mult_t<tiling_m44_base_mult_t> mult_m44;

    single_measure(matrix, matrixb, matrix, mult, size, enabled, 0);
    single_measure(matrix, matrixb, matrix, mult_22, size, enabled, 1);
    single_measure(matrix, matrixb, matrix, mult_44, size, enabled, 2);

    single_measure(matrix, matrixb, matrix, mult_m22, size, enabled, 3);
    single_measure(matrix, matrixb, matrix, mult_m24, size, enabled, 4);
    single_measure(matrix, matrixb, matrix, mult_m42, size, enabled, 5);
    single_measure(matrix, matrixb, matrix, mult_m35, size, enabled, 6);
    single_measure(matrix, matrixb, matrix, mult_m44, size, enabled, 7);

    dc_t                                           dc(4, 4);
    single_measure(dc, dc, dc, dgemm_t(), size, enabled, 8);

    gen_recursive_dense_mat_mat_mult_t<dgemm_add_t> mult_blas;
    single_measure(dc, dc, dc, mult_blas, size, enabled, 9);

    std::cout << "0\n";  std::cout.flush();
}

void measure_unrolling_hybrid(unsigned size, std::vector<int>& enabled)
{
    morton_dense<double,  doppled_64_row_mask>     d64r(4, 4);
    morton_dense<double,  doppled_64_col_mask>     d64c(4, 4);
    measure_unrolling(size, enabled, d64r, d64c);
}

void measure_unrolling_dense(unsigned size, std::vector<int>& enabled)
{
    dense2D<double> dense(4, 4);
    dense2D<double, mat::parameters<col_major> >    b(4, 4);
    measure_unrolling(size, enabled, dense, b);
}

void measure_orientation(unsigned size, std::vector<int>& enabled)
{
    std::cout << size << ", ";

    morton_dense<double,  doppled_64_row_mask>     d64r(4, 4);
    morton_dense<double,  doppled_64_col_mask>     d64c(4, 4);

    typedef gen_tiling_dense_mat_mat_mult_t<4, 2, ama_t>  tiling_m42_base_mult_t;
    gen_recursive_dense_mat_mat_mult_t<tiling_m42_base_mult_t> mult_m42;

    typedef gen_tiling_dense_mat_mat_mult_t<4, 4, ama_t>  tiling_m44_base_mult_t;
    gen_recursive_dense_mat_mat_mult_t<tiling_m44_base_mult_t> mult_m44;

    single_measure(d64r, d64r, d64r, mult_m42, size, enabled, 0);
    single_measure(d64c, d64c, d64c, mult_m42, size, enabled, 1);
    single_measure(d64r, d64c, d64r, mult_m42, size, enabled, 2);
    single_measure(d64c, d64r, d64r, mult_m42, size, enabled, 3);

    single_measure(d64r, d64r, d64r, mult_m44, size, enabled, 4);
    single_measure(d64c, d64c, d64c, mult_m44, size, enabled, 5);
    single_measure(d64r, d64c, d64r, mult_m44, size, enabled, 6);
    single_measure(d64c, d64r, d64r, mult_m44, size, enabled, 7);

    dc_t                                           dc(4, 4);
    single_measure(dc, dc, dc, dgemm_t(), size, enabled, 8);

    std::cout << "0\n";  std::cout.flush();
}

void measure_unrolling_32(unsigned size, std::vector<int>& enabled)
{
    std::cout << size << ", ";

    gen_recursive_dense_mat_mat_mult_t<base_mult_t, test32_t>           mult;
    gen_recursive_dense_mat_mat_mult_t<tiling_22_base_mult_t, test32_t> mult_22;
    gen_recursive_dense_mat_mat_mult_t<tiling_44_base_mult_t, test32_t> mult_44;

    typedef gen_tiling_dense_mat_mat_mult_t<2, 2, ama_t>  tiling_m22_base_mult_t;
    gen_recursive_dense_mat_mat_mult_t<tiling_m22_base_mult_t, test32_t> mult_m22;

    typedef gen_tiling_dense_mat_mat_mult_t<2, 4, ama_t>  tiling_m24_base_mult_t;
    gen_recursive_dense_mat_mat_mult_t<tiling_m24_base_mult_t, test32_t> mult_m24;

    typedef gen_tiling_dense_mat_mat_mult_t<4, 2, ama_t>  tiling_m42_base_mult_t;
    gen_recursive_dense_mat_mat_mult_t<tiling_m42_base_mult_t, test32_t> mult_m42;

    typedef gen_tiling_dense_mat_mat_mult_t<3, 5, ama_t>  tiling_m35_base_mult_t;
    gen_recursive_dense_mat_mat_mult_t<tiling_m35_base_mult_t, test32_t> mult_m35;

    typedef gen_tiling_dense_mat_mat_mult_t<4, 4, ama_t>  tiling_m44_base_mult_t;
    gen_recursive_dense_mat_mat_mult_t<tiling_m44_base_mult_t, test32_t> mult_m44;

    morton_dense<double,  doppled_32_row_mask>     d32r(4, 4);
    morton_dense<double,  doppled_32_col_mask>     d32c(4, 4);

    single_measure(d32r, d32c, d32r, mult, size, enabled, 0);
    single_measure(d32r, d32c, d32r, mult_22, size, enabled, 1);
    single_measure(d32r, d32c, d32r, mult_44, size, enabled, 2);

    single_measure(d32r, d32c, d32r, mult_m22, size, enabled, 3);
    single_measure(d32r, d32c, d32r, mult_m24, size, enabled, 4);
    single_measure(d32r, d32c, d32r, mult_m42, size, enabled, 5);
    single_measure(d32r, d32c, d32r, mult_m35, size, enabled, 6);
    single_measure(d32r, d32c, d32r, mult_m44, size, enabled, 7);

    dc_t                                           dc(4, 4);
    single_measure(dc, dc, dc, dgemm_t(), size, enabled, 8);

    std::cout << "0\n";  std::cout.flush();
}

void measure_hetero_value(unsigned size, std::vector<int>& enabled)
{
    using std::complex;
    typedef gen_tiling_dense_mat_mat_mult_t<4, 4, ama_t>  tiling_m44_base_mult_t;
    gen_recursive_dense_mat_mat_mult_t<tiling_m44_base_mult_t> mult;

    dc_t                                           dc(4, 4);
    dr_t                                           dr(4, 4);
    dense2D<float, mat::parameters<row_major> >  fr(4, 4);
    dense2D<float, mat::parameters<col_major> >  fc(4, 4);
    dense2D<complex<float>, mat::parameters<col_major> >   cc(4, 4);
    dense2D<complex<double>, mat::parameters<col_major> >  zc(4, 4);
    dense2D<complex<double>, mat::parameters<row_major> >  zr(4, 4);

    morton_dense<double,  doppled_64_row_mask>     d64r(4, 4);
    morton_dense<double,  doppled_64_col_mask>     d64c(4, 4);

    morton_dense<float,  doppled_64_row_mask>     f64r(4, 4);
    morton_dense<float,  doppled_64_col_mask>     f64c(4, 4);

    morton_dense<complex<float>,  doppled_64_row_mask>     c64r(4, 4);
    morton_dense<complex<float>,  doppled_64_col_mask>     c64c(4, 4);

    morton_dense<complex<double>,  doppled_64_row_mask>     z64r(4, 4);
    morton_dense<complex<double>,  doppled_64_col_mask>     z64c(4, 4);

    std::cout << size << ", ";

    single_measure(dr, fc, dr, mult, size, enabled, 0);
    single_measure(fr, fc, dr, mult, size, enabled, 1);

    //single_measure(fr, zc, zr, mult, size, enabled, 2);
    //single_measure(dr, cc, zr, mult, size, enabled, 3);

    single_measure(d64r, f64c, d64r, mult, size, enabled, 4);
    single_measure(d64r, z64c, z64r, mult, size, enabled, 5);
    std::cout << "0\n";  std::cout.flush();
}

void measure_hetero_layout(unsigned size, std::vector<int>& enabled)
{
    using std::complex;
    typedef gen_tiling_dense_mat_mat_mult_t<4, 4, ama_t>  tiling_m44_base_mult_t;
    gen_recursive_dense_mat_mat_mult_t<tiling_m44_base_mult_t> mult;

    dc_t                                           dc(4, 4);
    dr_t                                           dr(4, 4);
    dense2D<float, mat::parameters<row_major> >  fr(4, 4);
    dense2D<float, mat::parameters<col_major> >  fc(4, 4);
    dense2D<complex<float>, mat::parameters<col_major> >   cc(4, 4);
    dense2D<complex<double>, mat::parameters<col_major> >  zc(4, 4);
    dense2D<complex<double>, mat::parameters<row_major> >  zr(4, 4);

    morton_dense<double,  doppled_64_row_mask>     d64r(4, 4);
    morton_dense<double,  doppled_64_col_mask>     d64c(4, 4);

    morton_dense<float,  doppled_64_row_mask>     f64r(4, 4);
    morton_dense<float,  doppled_64_col_mask>     f64c(4, 4);

    morton_dense<complex<float>,  doppled_64_row_mask>     c64r(4, 4);
    morton_dense<complex<float>,  doppled_64_col_mask>     c64c(4, 4);

    morton_dense<complex<double>,  doppled_64_row_mask>     z64r(4, 4);
    morton_dense<complex<double>,  doppled_64_col_mask>     z64c(4, 4);

    std::cout << size << ", ";

    single_measure(dr, f64c, dr, mult, size, enabled, 0);
    single_measure(fr, f64c, dr, mult, size, enabled, 1);

    //single_measure(f64r, zc, zr, mult, size, enabled, 2);
    single_measure(d64r, zc, zr, mult, size, enabled, 3);

    single_measure(d64r, f64c, dr, mult, size, enabled, 4);
    //single_measure(f64r, z64c, zr, mult, size, enabled, 5);
    std::cout << "0\n";  std::cout.flush();
}

template <typename Measure>
void series(unsigned steps, unsigned max_size, Measure measure, const string& comment)
{
    std::cout << "# " << comment << '\n';
    std::cout << "# Gnu-Format size, (time, MFlops, L1, L2, TLB, time, MFlops, )*\n"; std::cout.flush();

    std::vector<int> enabled(16, 1);
    for (unsigned i= steps; i <= max_size; i+= steps)
	measure(i, enabled);
}

int main(int argc, char* argv[])
{
    papi.start();

    std::vector<std::string> scenarii;
    scenarii.push_back(string("Comparing Z-, N-order and mixed with recursive multiplication"));
    scenarii.push_back(string("Comparing base case cast (64) for Z-order, hybrid 32, hybrid 64 row and col-major"));
    scenarii.push_back(string("Using unrolled mult on hybrid row- and column-major matrices"));
    scenarii.push_back(string("Comparing base case sizes for corresponding hybrid row-major matrices"));
    scenarii.push_back(string("Comparing different unrolling for hybrid row-major matrices"));
    scenarii.push_back(string("Comparing different unrolling for row-major dense matrices"));
    scenarii.push_back(string("Comparing different orientations for hybrid row-major matrices"));
    scenarii.push_back(string("Comparing different unrolling for hybrid 32 row-major times col-major matrices"));
    scenarii.push_back(string("Multiplying matrices with different value types"));
    scenarii.push_back(string("Multiplying matrices with different matrix layouts"));

    using std::cout;
    if (argc < 4) {
	cerr << "usage: recursive_mult_timing <scenario> <steps> <max_size>\nScenarii:\n";
	for (unsigned i= 0; i < scenarii.size(); i++)
	    cout << i << ": " << scenarii[i] << "\n";
	exit(1);
    }
    unsigned int scenario= atoi(argv[1]), steps= atoi(argv[2]), max_size= atoi(argv[3]), size= 32;

    switch (scenario) {
      case 0: 	series(steps, max_size, measure_morton_order, scenarii[0]); break;
      case 1: 	series(steps, max_size, measure_cast, scenarii[1]); break;
      case 2: 	series(steps, max_size, measure_with_unroll, scenarii[2]); break;
      case 3: 	series(steps, max_size, measure_base_size, scenarii[3]); break;
      case 4: 	series(steps, max_size, measure_unrolling_hybrid, scenarii[4]); break;
      case 5: 	series(steps, max_size, measure_unrolling_dense, scenarii[5]); break;
      case 6: 	series(steps, max_size, measure_orientation, scenarii[6]); break;
      case 7: 	series(steps, max_size, measure_unrolling_32, scenarii[7]); break;
      case 8: 	series(steps, max_size, measure_hetero_value, scenarii[8]); break;
      case 9: 	series(steps, max_size, measure_hetero_layout, scenarii[9]); break;
    }

    return 0;

}

#if 0

// scheiss Kommandos

// g++4 matrix_product_timing.cpp  -o matrix_product_timing -O3 -DNDEBUG -ffast-math  -mcpu=opteron -mtune=opteron -msse2 -mfpmath=sse -I${MTL_BOOST_ROOT} -I${BOOST_ROOT} -I/usr/local/include -L/usr/local/lib -lpapi -DMTL_HAS_PAPI -DMTL_HAS_BLAS  -L/u/htor/projekte/mathlibs/goto-blas -lgoto_opteron-64 -lpthread xerbla.o

// g++4 matrix_product_timing.cpp  -o matrix_product_timing -O3 -DNDEBUG -ffast-math  -mcpu=opteron -mtune=opteron -msse2 -mfpmath=sse -I${MTL_BOOST_ROOT} -I${BOOST_ROOT} -I/usr/local/include -L/usr/local/lib -lpapi -DMTL_HAS_PAPI -DMTL_HAS_BLAS  -L/u/htor/projekte/mathlibs/acml-2-6-0-gnu-64bit/gnu64/lib -lacml -L/usr/lib/gcc/x86_64-redhat-linux/3.4.3 -lg2c

// g++4 matrix_product_timing.cpp -o matrix_product_timing -O3 -DNDEBUG -ffast-math -mcpu=opteron -mtune=opteron -msse2 -mfpmath=sse -I${MTL_BOOST_ROOT} -I${BOOST_ROOT} -I/usr/local/include -L/usr/local/lib -lpapi -DMTL_HAS_PAPI -DMTL_HAS_BLAS -L/san/atlas/lib -lf77blas -latlas -L/usr/lib/gcc/x86_64-redhat-linux/3.4.3 -lg2c

#endif


#pragma once

#include "config.h"
#include <vector>
#include <memory>
#include <random>
#include <boost/circular_buffer.hpp>

#include "qconfig.hpp"
#include "action.h"

namespace net
{
	class Solver;
	class ComputationGraph;
}

namespace qlearn
{
	class MemoryCache;

	class QCore
	{
	public:

		QCore( Config cfg );
		~QCore( );

		std::size_t getRandomAction();

		// propagate a game state forward through th graph, and get the action according to the current policy.
		Action forward( net::ComputationGraph& policy, const Vector& input, bool learn = true );

		// save the result of the action that was propagated by forward.
		void backward( float reward, bool terminal );

		// accumulates gradients of policy in the solver.
		// returns mse of minibatch.
		float learn(net::ComputationGraph& policy, net::ComputationGraph& target, net::Solver& solver);

		std::size_t getSteps() const { return mStepCounter; }
		float getEpsilon() const;

	private:
		Config mConfig;

		std::unique_ptr<MemoryCache> mMemory;
		std::size_t mStepCounter = 0;
		std::size_t mLearningSteps = 0;

		// cache the last situations
		boost::circular_buffer<Vector> mLastStates;
		boost::circular_buffer<float> mLastRewards;
		boost::circular_buffer<bool> mLastTerminal;
		boost::circular_buffer<std::size_t> mLastActions;

		Vector mErrorCache; // cache vector to prevent reallocation

		// random engine
		std::default_random_engine mRandom;
	};
}


//  Copyright (c) 2015 Daniel Bourgeois
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

/// \file parallel/algorithms/inner_product.hpp

#if !defined(HPX_PARALLEL_ALGORITHM_INNER_PRODUCT_JUL_15_2015_0730AM)
#define HPX_PARALLEL_ALGORITHM_INNER_PRODUCT_JUL_15_2015_0730AM

#include <hpx/hpx_fwd.hpp>
#include <hpx/util/move.hpp>
#include <hpx/util/zip_iterator.hpp>

#include <hpx/parallel/config/inline_namespace.hpp>
#include <hpx/parallel/execution_policy.hpp>
#include <hpx/parallel/algorithms/detail/dispatch.hpp>
#include <hpx/parallel/util/detail/algorithm_result.hpp>
#include <hpx/parallel/util/partitioner.hpp>
#include <hpx/parallel/util/loop.hpp>

#include <algorithm>
#include <numeric>
#include <iterator>

#include <boost/static_assert.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_base_of.hpp>

namespace hpx { namespace parallel { HPX_INLINE_NAMESPACE(v1)
{
    ///////////////////////////////////////////////////////////////////////////
    // inner_product
    namespace detail
    {
        template <typename T>
        struct inner_product
          : public detail::algorithm<inner_product<T>, T>
        {
            inner_product()
              : inner_product::algorithm("inner_product")
            {}

            template <typename ExPolicy, typename InIter1, typename InIter2,
                typename T_, typename Op1, typename Op2>
            static T
            sequential(ExPolicy, InIter1 first1, InIter1 last1, InIter2 first2,
                T_ && init, Op1 && op1, Op2 && op2)
            {
                return std::inner_product(
                    first1, last1, first2, std::forward<T_>(init),
                    std::forward<Op1>(op1), std::forward<Op2>(op2));
            }

            template <typename ExPolicy, typename FwdIter1, typename FwdIter2,
                typename T_, typename Op1, typename Op2>
            static typename util::detail::algorithm_result<
                ExPolicy, T
            >::type
            parallel(ExPolicy policy, FwdIter1 first1, FwdIter1 last1,
                 FwdIter2 first2, T_ && init, Op1 && op1, Op2 && op2)
            {
                typedef util::detail::algorithm_result<ExPolicy, T> result;
                typedef hpx::util::zip_iterator<FwdIter1, FwdIter2>
                    zip_iterator;
                typedef typename std::iterator_traits<FwdIter1>::difference_type
                    difference_type;

                if (first1 == last1)
                    return result::get(std::forward<T_>(init));

                difference_type count = std::distance(first1, last1);

                using hpx::util::make_zip_iterator;
                return util::partitioner<ExPolicy, T>::call(
                    policy, make_zip_iterator(first1, first2), count,
                    [op1, op2](zip_iterator part_begin, std::size_t part_size) ->T
                    {
                        using hpx::util::get;
                        T part_sum = op2(
                            get<0>(*part_begin), get<1>(*part_begin));
                        part_begin++;

                        // VS2015RC bails out when op is captured by ref
                        util::loop_n(part_begin, part_size - 1,
                            [=, &part_sum](zip_iterator it)
                            {
                                part_sum = op1(
                                    part_sum, op2(get<0>(*it), get<1>(*it)));
                            });
                        return part_sum;
                    },
                    [init, op1](std::vector<hpx::future<T> > && results) -> T
                    {
                        T ret = init;
                        for(auto && fut : results)
                        {
                            ret = op1(ret, fut.get());
                        }
                        return ret;
                    });
            }
        };
        /// \endcond
    }

    ///////////////////////////////////////////////////////////////////////////
    /// Returns the result of accumulating init with the inner products of the
    /// pairs formed by the elements of two ranges starting at first1 and
    /// first2.
    ///
    /// \note   Complexity: O(\a last - \a first) applications of the
    ///         predicate \a op2.
    /// \tparam ExPolicy    The type of the execution policy to use (deduced).
    ///                     It describes the manner in which the execution
    ///                     of the algorithm may be parallelized and the manner
    ///                     in which it executes the assignments.
    /// \tparam InIter1     The type of the first source iterators used
    ///                     (deduced). This iterator type must meet the
    ///                     requirements of an input iterator.
    /// \tparam InIter2     The type of the second source iterators used
    ///                     (deduced). This iterator type must meet the
    ///                     requirements of an input iterator.
    /// \tparam OutIter     The type of the iterator representing the
    ///                     destination range (deduced).
    ///                     This iterator type must meet the requirements of an
    ///                     output iterator.
    /// \tparam T           The type of the value to be used as return)
    ///                     values (deduced).
    /// \tparam Op1         The type of the binary function object used for
    ///                     the summation operation.
    /// \tparam Op2         The type of the binary function object used for
    ///                     the multiplication operation.
    ///
    /// \param policy       The execution policy to use for the scheduling of
    ///                     the iterations.
    /// \param first1       Refers to the beginning of the first sequence of
    ///                     elements the result will be calculated with.
    /// \param last1        Refers to the end of the first sequence of elements
    ///                     the algorithm will be applied to.
    /// \param first2       Refers to the beginning of the second sequence of
    ///                     elements the result will be calculated with.
    /// \param init         The initial value for the sum.
    ///
    /// The operations in the parallel \a inner_product algorithm invoked
    /// with an execution policy object of type \a sequential_execution_policy
    /// execute in sequential order in the calling thread.
    ///
    /// The operations in the parallel \a inner_product algorithm invoked
    /// with an execution policy object of type \a parallel_execution_policy
    /// or \a parallel_task_execution_policy are permitted to execute in an unordered
    /// fashion in unspecified threads, and indeterminately sequenced
    /// within each thread.
    ///
    /// \returns  The \a inner_product algorithm returns a \a hpx::future<T> if
    ///           the execution policy is of type
    ///           \a sequential_task_execution_policy or
    ///           \a parallel_task_execution_policy and
    ///           returns \a OutIter otherwise.
    ///

    template <typename ExPolicy, typename InIter1, typename InIter2, typename T>
    inline typename boost::enable_if<
        is_execution_policy<ExPolicy>,
        typename util::detail::algorithm_result<ExPolicy, T>::type
    >::type
    inner_product(ExPolicy&& policy, InIter1 first1, InIter1 last1,
        InIter2 first2, T init)
    {
        typedef typename std::iterator_traits<InIter1>::iterator_category
            iterator_category_1;
        typedef typename std::iterator_traits<InIter2>::iterator_category
            iterator_category_2;

        BOOST_STATIC_ASSERT_MSG(
            (boost::is_base_of<std::input_iterator_tag,
                iterator_category_1>::value),
            "Requires at least input iterator.");
        BOOST_STATIC_ASSERT_MSG(
            (boost::is_base_of<std::input_iterator_tag,
                iterator_category_2>::value),
            "Requires at least input iterator.");

        typedef typename boost::mpl::or_<
            is_sequential_execution_policy<ExPolicy>,
            boost::is_same<std::input_iterator_tag, iterator_category_1>,
            boost::is_same<std::input_iterator_tag, iterator_category_2>
        >::type is_seq;

        return detail::inner_product<T>().call(
            std::forward<ExPolicy>(policy), is_seq(), first1, last1, first2,
            std::move(init), std::plus<T>(), std::multiplies<T>());
    }

    ///////////////////////////////////////////////////////////////////////////
    /// Returns the result of accumulating init with the inner products of the
    /// pairs formed by the elements of two ranges starting at first1 and
    /// first2.
    ///
    /// \note   Complexity: O(\a last - \a first) applications of the
    ///         predicate \a op2.
    /// \tparam ExPolicy    The type of the execution policy to use (deduced).
    ///                     It describes the manner in which the execution
    ///                     of the algorithm may be parallelized and the manner
    ///                     in which it executes the assignments.
    /// \tparam InIter1     The type of the first source iterators used
    ///                     (deduced). This iterator type must meet the
    ///                     requirements of an input iterator.
    /// \tparam InIter2     The type of the second source iterators used
    ///                     (deduced). This iterator type must meet the
    ///                     requirements of an input iterator.
    /// \tparam OutIter     The type of the iterator representing the
    ///                     destination range (deduced).
    ///                     This iterator type must meet the requirements of an
    ///                     output iterator.
    /// \tparam T           The type of the value to be used as return)
    ///                     values (deduced).
    /// \tparam Op1         The type of the binary function object used for
    ///                     the summation operation.
    /// \tparam Op2         The type of the binary function object used for
    ///                     the multiplication operation.
    ///
    /// \param policy       The execution policy to use for the scheduling of
    ///                     the iterations.
    /// \param first1       Refers to the beginning of the first sequence of
    ///                     elements the result will be calculated with.
    /// \param last1        Refers to the end of the first sequence of elements
    ///                     the algorithm will be applied to.
    /// \param first2       Refers to the beginning of the second sequence of
    ///                     elements the result will be calculated with.
    /// \param init         The initial value for the sum.
    /// \param op1          Specifies the function (or function object) which
    ///                     will be invoked for each of the input values
    ///                     of the sequence. This is a binary predicate. The
    ///                     signature of this predicate should be equivalent to
    ///                     \code
    ///                     Ret fun(const Type1 &a, const Type2 &b);
    ///                     \endcode \n
    ///                     The signature does not need to have const&, but
    ///                     the function must not modify the objects passed to
    ///                     it.
    ///                     The type \a Ret must be such that it can be
    ///                     implicitly converted to an object for the second
    ///                     argument type of \a op2.
    /// \param op2          Specifies the function (or function object) which
    ///                     will be invoked for the initial value and each
    ///                     of the return values of \a op1.
    ///                     This is a binary predicate. The
    ///                     signature of this predicate should be equivalent to
    ///                     should be equivalent to:
    ///                     \code
    ///                     Ret fun(const Type1 &a, const Type1 &b);
    ///                     \endcode \n
    ///                     The signature does not need to have const&, but
    ///                     the function must not modify the objects passed to
    ///                     it.
    ///                     The type \a Ret must be
    ///                     such that it can be implicitly converted to a type
    ///                     of \t.
    ///
    /// The operations in the parallel \a inner_product algorithm invoked
    /// with an execution policy object of type \a sequential_execution_policy
    /// execute in sequential order in the calling thread.
    ///
    /// The operations in the parallel \a inner_product algorithm invoked
    /// with an execution policy object of type \a parallel_execution_policy
    /// or \a parallel_task_execution_policy are permitted to execute in an unordered
    /// fashion in unspecified threads, and indeterminately sequenced
    /// within each thread.
    ///
    /// \returns  The \a inner_product algorithm returns a \a hpx::future<T> if
    ///           the execution policy is of type
    ///           \a sequential_task_execution_policy or
    ///           \a parallel_task_execution_policy and
    ///           returns \a OutIter otherwise.
    ///

    template <typename ExPolicy, typename InIter1, typename InIter2, typename T,
        typename Op1, typename Op2>
    inline typename boost::enable_if<
        is_execution_policy<ExPolicy>,
        typename util::detail::algorithm_result<ExPolicy, T>::type
    >::type
    inner_product(ExPolicy&& policy, InIter1 first1, InIter1 last1,
        InIter2 first2, T init, Op1 && op1, Op2 && op2)
    {
        typedef typename std::iterator_traits<InIter1>::iterator_category
            iterator_category_1;
        typedef typename std::iterator_traits<InIter2>::iterator_category
            iterator_category_2;

        BOOST_STATIC_ASSERT_MSG(
            (boost::is_base_of<std::input_iterator_tag,
                iterator_category_1>::value),
            "Requires at least input iterator.");
        BOOST_STATIC_ASSERT_MSG(
            (boost::is_base_of<std::input_iterator_tag,
                iterator_category_2>::value),
            "Requires at least input iterator.");

        typedef typename boost::mpl::or_<
            is_sequential_execution_policy<ExPolicy>,
            boost::is_same<std::input_iterator_tag, iterator_category_1>,
            boost::is_same<std::input_iterator_tag, iterator_category_2>
        >::type is_seq;

        return detail::inner_product<T>().call(
            std::forward<ExPolicy>(policy), is_seq(), first1, last1, first2,
            std::move(init), std::forward<Op1>(op1), std::forward<Op2>(op2));
    }

}}}

#endif