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


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


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#ifndef BOOST_MATH_FLOAT_BACKEND_HPP
#define BOOST_MATH_FLOAT_BACKEND_HPP

#include <iostream>
#include <iomanip>
#include <sstream>
#include <cstdint>
#include <boost/lexical_cast.hpp>
#include <boost/math/concepts/real_concept.hpp>
#include <boost/multiprecision/number.hpp>
#include <boost/integer/common_factor_rt.hpp>
#include <boost/container_hash/hash.hpp>

namespace boost {
namespace multiprecision {
namespace backends {

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable : 4389 4244 4018 4244 4127)
#endif

template <class Arithmetic>
struct arithmetic_backend
{
   typedef std::tuple<short, int, long, long long>                                 signed_types;
   typedef std::tuple<unsigned short, unsigned, unsigned long, unsigned long long> unsigned_types;
   typedef std::tuple<float, double, long double>                                  float_types;
   typedef int                                                                    exponent_type;

   BOOST_MP_CXX14_CONSTEXPR arithmetic_backend() : m_value(0) {}
   BOOST_MP_CXX14_CONSTEXPR arithmetic_backend(const arithmetic_backend& o) : m_value(o.m_value) {}
   template <class A>
   BOOST_MP_CXX14_CONSTEXPR arithmetic_backend(const A& o, const typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A>::value >::type* = 0) : m_value(o) {}
   template <class A>
   BOOST_MP_CXX14_CONSTEXPR arithmetic_backend(const arithmetic_backend<A>& o) : m_value(o.data()) {}
   BOOST_MP_CXX14_CONSTEXPR arithmetic_backend& operator=(const arithmetic_backend& o)
   {
      m_value = o.m_value;
      return *this;
   }
   template <class A>
   BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A>::value, arithmetic_backend&>::type operator=(A i)
   {
      m_value = static_cast<Arithmetic>(i);
      return *this;
   }
   template <class A>
   BOOST_MP_CXX14_CONSTEXPR arithmetic_backend& operator=(const arithmetic_backend<A>& i)
   {
      m_value = i.data();
      return *this;
   }
   arithmetic_backend& operator=(const char* s)
   {
#ifndef BOOST_NO_EXCEPTIONS
      try
      {
#endif
         m_value = boost::lexical_cast<Arithmetic>(s);
#ifndef BOOST_NO_EXCEPTIONS
      }
      catch (const bad_lexical_cast&)
      {
         throw std::runtime_error(std::string("Unable to interpret the string provided: \"") + s + std::string("\" as a compatible number type."));
      }
#endif
      return *this;
   }
   BOOST_MP_CXX14_CONSTEXPR void swap(arithmetic_backend& o)
   {
      std::swap(m_value, o.m_value);
   }
   std::string str(std::streamsize digits, std::ios_base::fmtflags f) const
   {
      std::stringstream ss;
      ss.flags(f);
      ss << std::setprecision(digits ? digits : std::numeric_limits<Arithmetic>::digits10 + 4) << m_value;
      return ss.str();
   }
   BOOST_MP_CXX14_CONSTEXPR void do_negate(const std::integral_constant<bool, true>&)
   {
      m_value = 1 + ~m_value;
   }
   BOOST_MP_CXX14_CONSTEXPR void do_negate(const std::integral_constant<bool, false>&)
   {
      m_value = -m_value;
   }
   BOOST_MP_CXX14_CONSTEXPR void negate()
   {
      do_negate(std::integral_constant<bool, boost::multiprecision::detail::is_unsigned<Arithmetic>::value>());
   }
   BOOST_MP_CXX14_CONSTEXPR int compare(const arithmetic_backend& o) const
   {
      return m_value > o.m_value ? 1 : (m_value < o.m_value ? -1 : 0);
   }
   template <class A>
   BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A>::value, int>::type compare(A i) const
   {
      return m_value > static_cast<Arithmetic>(i) ? 1 : (m_value < static_cast<Arithmetic>(i) ? -1 : 0);
   }
   BOOST_MP_CXX14_CONSTEXPR Arithmetic& data() { return m_value; }
   BOOST_MP_CXX14_CONSTEXPR const Arithmetic& data() const { return m_value; }

 private:
   Arithmetic m_value;
};

template <class R, class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_integral<R>::value>::type eval_convert_to(R* result, const arithmetic_backend<Arithmetic>& backend)
{
   typedef typename std::common_type<R, Arithmetic>::type c_type;
   constexpr const c_type                             max = static_cast<c_type>((std::numeric_limits<R>::max)());
   constexpr const c_type                             min = static_cast<c_type>((std::numeric_limits<R>::min)());
   c_type                                                   ct  = static_cast<c_type>(backend.data());
   if ((backend.data() < 0) && !std::numeric_limits<R>::is_signed)
      BOOST_THROW_EXCEPTION(std::range_error("Attempt to convert negative number to unsigned type."));
   if (ct > max)
      *result = boost::multiprecision::detail::is_signed<R>::value ? (std::numeric_limits<R>::max)() : backend.data();
   else if (std::numeric_limits<Arithmetic>::is_signed && (ct < min))
      *result = (std::numeric_limits<R>::min)();
   else
      *result = backend.data();
}

template <class R, class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<!boost::multiprecision::detail::is_integral<R>::value && !std::is_enum<R>::value>::type eval_convert_to(R* result, const arithmetic_backend<Arithmetic>& backend)
{
   *result = backend.data();
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR bool eval_eq(const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   return a.data() == b.data();
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value, bool>::type eval_eq(const arithmetic_backend<Arithmetic>& a, const A2& b)
{
   return a.data() == static_cast<Arithmetic>(b);
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR bool eval_lt(const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   return a.data() < b.data();
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value, bool>::type eval_lt(const arithmetic_backend<Arithmetic>& a, const A2& b)
{
   return a.data() < static_cast<Arithmetic>(b);
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR bool eval_gt(const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   return a.data() > b.data();
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value, bool>::type eval_gt(const arithmetic_backend<Arithmetic>& a, const A2& b)
{
   return a.data() > static_cast<Arithmetic>(b);
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_add(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   result.data() += o.data();
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_subtract(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   result.data() -= o.data();
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_multiply(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   result.data() *= o.data();
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<std::numeric_limits<Arithmetic>::has_infinity>::type eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   result.data() /= o.data();
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<!std::numeric_limits<Arithmetic>::has_infinity>::type eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   if (!o.data())
      BOOST_THROW_EXCEPTION(std::overflow_error("Divide by zero"));
   result.data() /= o.data();
}

template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value>::type eval_add(arithmetic_backend<Arithmetic>& result, const A2& o)
{
   result.data() += o;
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value>::type eval_subtract(arithmetic_backend<Arithmetic>& result, const A2& o)
{
   result.data() -= o;
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value>::type eval_multiply(arithmetic_backend<Arithmetic>& result, const A2& o)
{
   result.data() *= o;
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<(boost::multiprecision::detail::is_arithmetic<A2>::value && !std::numeric_limits<Arithmetic>::has_infinity)>::type
eval_divide(arithmetic_backend<Arithmetic>& result, const A2& o)
{
   if (!o)
      BOOST_THROW_EXCEPTION(std::overflow_error("Divide by zero"));
   result.data() /= o;
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<(boost::multiprecision::detail::is_arithmetic<A2>::value && std::numeric_limits<Arithmetic>::has_infinity)>::type
eval_divide(arithmetic_backend<Arithmetic>& result, const A2& o)
{
   result.data() /= o;
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_add(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   result.data() = a.data() + b.data();
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_subtract(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   result.data() = a.data() - b.data();
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_multiply(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   result.data() = a.data() * b.data();
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<std::numeric_limits<Arithmetic>::has_infinity>::type eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   result.data() = a.data() / b.data();
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<!std::numeric_limits<Arithmetic>::has_infinity>::type eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   if (!b.data())
      BOOST_THROW_EXCEPTION(std::overflow_error("Divide by zero"));
   result.data() = a.data() / b.data();
}

template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value>::type eval_add(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const A2& b)
{
   result.data() = a.data() + b;
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value>::type eval_subtract(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const A2& b)
{
   result.data() = a.data() - b;
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value>::type eval_multiply(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const A2& b)
{
   result.data() = a.data() * b;
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<(boost::multiprecision::detail::is_arithmetic<A2>::value && !std::numeric_limits<Arithmetic>::has_infinity)>::type
eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const A2& b)
{
   if (!b)
      BOOST_THROW_EXCEPTION(std::overflow_error("Divide by zero"));
   result.data() = a.data() / b;
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<(boost::multiprecision::detail::is_arithmetic<A2>::value && std::numeric_limits<Arithmetic>::has_infinity)>::type
eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const A2& b)
{
   result.data() = a.data() / b;
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR bool eval_is_zero(const arithmetic_backend<Arithmetic>& val)
{
   return val.data() == 0;
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<
    (!std::numeric_limits<Arithmetic>::is_specialized || std::numeric_limits<Arithmetic>::is_signed), int>::type
eval_get_sign(const arithmetic_backend<Arithmetic>& val)
{
   return val.data() == 0 ? 0 : val.data() < 0 ? -1 : 1;
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<
    !(std::numeric_limits<Arithmetic>::is_specialized || std::numeric_limits<Arithmetic>::is_signed), int>::type
eval_get_sign(const arithmetic_backend<Arithmetic>& val)
{
   return val.data() == 0 ? 0 : 1;
}

template <class T>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_unsigned<T>::value, T>::type abs(T v) { return v; }

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_abs(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   using boost::multiprecision::backends::abs;
   using std::abs;
   result.data() = abs(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_fabs(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   result.data() = std::abs(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_floor(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = floor(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_ceil(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = ceil(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_sqrt(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = sqrt(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR int eval_fpclassify(const arithmetic_backend<Arithmetic>& o)
{
   return (boost::math::fpclassify)(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_trunc(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = trunc(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_round(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = round(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_frexp(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, int* v)
{
   BOOST_MATH_STD_USING
   result.data() = frexp(a.data(), v);
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_ldexp(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, int v)
{
   BOOST_MATH_STD_USING
   result.data() = ldexp(a.data(), v);
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_exp(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = exp(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_log(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = log(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_log10(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = log10(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_sin(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = sin(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_cos(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = cos(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_tan(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = tan(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_acos(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = acos(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_asin(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = asin(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_atan(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = atan(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_sinh(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = sinh(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_cosh(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = cosh(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_tanh(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = tanh(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_fmod(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   BOOST_MATH_STD_USING
   result.data() = fmod(a.data(), b.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_pow(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   BOOST_MATH_STD_USING
   result.data() = pow(a.data(), b.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_atan2(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   BOOST_MATH_STD_USING
   result.data() = atan2(a.data(), b.data());
}

template <class Arithmetic, class I>
inline BOOST_MP_CXX14_CONSTEXPR void eval_left_shift(arithmetic_backend<Arithmetic>& result, I val)
{
   result.data() <<= val;
}

template <class Arithmetic, class I>
inline BOOST_MP_CXX14_CONSTEXPR void eval_right_shift(arithmetic_backend<Arithmetic>& result, I val)
{
   result.data() >>= val;
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_modulus(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a)
{
   result.data() %= a.data();
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_bitwise_and(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a)
{
   result.data() &= a.data();
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_bitwise_or(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a)
{
   result.data() |= a.data();
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_bitwise_xor(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a)
{
   result.data() ^= a.data();
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_complement(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a)
{
   result.data() = ~a.data();
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_gcd(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   result.data() = boost::integer::gcd(a.data(), b.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_lcm(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   result.data() = boost::integer::lcm(a.data(), b.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR std::size_t hash_value(const arithmetic_backend<Arithmetic>& a)
{
   boost::hash<Arithmetic> hasher;
   return hasher(a.data());
}

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

} // namespace backends

using boost::multiprecision::backends::arithmetic_backend;

template <class Arithmetic>
struct number_category<arithmetic_backend<Arithmetic> > : public std::integral_constant<int, boost::multiprecision::detail::is_integral<Arithmetic>::value ? number_kind_integer : number_kind_floating_point>
{};

namespace detail {

template <class Backend>
struct double_precision_type;

template <class Arithmetic, boost::multiprecision::expression_template_option ET>
struct double_precision_type<number<arithmetic_backend<Arithmetic>, ET> >
{
   typedef number<arithmetic_backend<typename double_precision_type<Arithmetic>::type>, ET> type;
};
template <>
struct double_precision_type<arithmetic_backend<std::int32_t> >
{
   typedef arithmetic_backend<std::int64_t> type;
};

} // namespace detail

}} // namespace boost::multiprecision
#if !(defined(__SGI_STL_PORT) || defined(BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS))
//
// We shouldn't need these to get code to compile, however for the sake of
// "level playing field" performance comparisons they avoid the very slow
// lexical_cast's that would otherwise take place.  Definition has to be guarded
// by the inverse of pp-logic in real_concept.hpp which defines these as a workaround
// for STLPort plus some other old/broken standartd libraries.
//
namespace boost { namespace math { namespace tools {

template <>
inline unsigned int real_cast<unsigned int, concepts::real_concept>(concepts::real_concept r)
{
   return static_cast<unsigned int>(r.value());
}

template <>
inline int real_cast<int, concepts::real_concept>(concepts::real_concept r)
{
   return static_cast<int>(r.value());
}

template <>
inline long real_cast<long, concepts::real_concept>(concepts::real_concept r)
{
   return static_cast<long>(r.value());
}

// Converts from T to narrower floating-point types, float, double & long double.

template <>
inline float real_cast<float, concepts::real_concept>(concepts::real_concept r)
{
   return static_cast<float>(r.value());
}
template <>
inline double real_cast<double, concepts::real_concept>(concepts::real_concept r)
{
   return static_cast<double>(r.value());
}
template <>
inline long double real_cast<long double, concepts::real_concept>(concepts::real_concept r)
{
   return r.value();
}

}}} // namespace boost::math::tools
#endif

namespace std {

template <class Arithmetic, boost::multiprecision::expression_template_option ExpressionTemplates>
class numeric_limits<boost::multiprecision::number<boost::multiprecision::arithmetic_backend<Arithmetic>, ExpressionTemplates> > : public std::numeric_limits<Arithmetic>
{
   typedef std::numeric_limits<Arithmetic>                                                                           base_type;
   typedef boost::multiprecision::number<boost::multiprecision::arithmetic_backend<Arithmetic>, ExpressionTemplates> number_type;

 public:
   static constexpr number_type(min)() noexcept { return (base_type::min)(); }
   static constexpr number_type(max)() noexcept { return (base_type::max)(); }
   static constexpr number_type lowest() noexcept { return -(max)(); }
   static constexpr number_type epsilon() noexcept { return base_type::epsilon(); }
   static constexpr number_type round_error() noexcept { return epsilon() / 2; }
   static constexpr number_type infinity() noexcept { return base_type::infinity(); }
   static constexpr number_type quiet_NaN() noexcept { return base_type::quiet_NaN(); }
   static constexpr number_type signaling_NaN() noexcept { return base_type::signaling_NaN(); }
   static constexpr number_type denorm_min() noexcept { return base_type::denorm_min(); }
};

template <>
class numeric_limits<boost::math::concepts::real_concept> : public std::numeric_limits<long double>
{
   typedef std::numeric_limits<long double>    base_type;
   typedef boost::math::concepts::real_concept number_type;

 public:
   static const number_type(min)() noexcept { return (base_type::min)(); }
   static const number_type(max)() noexcept { return (base_type::max)(); }
   static const number_type lowest() noexcept { return -(max)(); }
   static const number_type epsilon() noexcept { return base_type::epsilon(); }
   static const number_type round_error() noexcept { return epsilon() / 2; }
   static const number_type infinity() noexcept { return base_type::infinity(); }
   static const number_type quiet_NaN() noexcept { return base_type::quiet_NaN(); }
   static const number_type signaling_NaN() noexcept { return base_type::signaling_NaN(); }
   static const number_type denorm_min() noexcept { return base_type::denorm_min(); }
};

} // namespace std

#include <boost/multiprecision/detail/integer_ops.hpp>

#endif


#include <byteswap.h>
#include <stdio.h>
#include <unistd.h>
#include <vector>
#include <Eigen/Dense>
#include "imu/imu.h"

using Eigen::Matrix4f;
using Eigen::Vector3f;
using Eigen::Vector4f;

namespace {
const uint8_t ADDR_ITG3200  = 0x68;
const uint8_t ADDR_HMC5883L = 0x1e;
const uint8_t ADDR_ADXL345  = 0x53;

const Vector3f gyrocal_b(-182.11, -179.32, -212.83);
const Vector3f gyrocal_m(-0.01169, -0.01123, -0.01164);
const float pointcloud_dist = 200.0f;

bool CheckPointCloud(const Vector3f& pt, std::vector<Vector3f> *pointcloud) {
  for (int i = 0; i < pointcloud->size(); i++) {
    if (((*pointcloud)[i] - pt).squaredNorm() <
        pointcloud_dist*pointcloud_dist) {
      return false;
    }
  }
  pointcloud->push_back(pt);
  return true;
}

}  // empty namespace

// our motion model should figure out whether the car is on the ground (1g down
// on the accelerometer) or flying off a ramp (0g); if it's on the ground and
// the motor is stopped (0 estimated velocity) then we can zero out the gyro,
// and calibrate the direction of the accelerometer Y direction.

bool IMU::Init() {
  mag_XTX_ = 0.01 * Matrix4f::Identity();
  mag_XTY_ = Vector4f::Zero();
  mag_bias_ = Vector3f::Zero();

  // config gyro
  i2c_.Write(ADDR_ITG3200, 0x3E, 0x01);  // use X gyro PLL oscillator
  i2c_.Write(ADDR_ITG3200, 0x15, 19);    // samplerate 50Hz (1000/(19+1))
  i2c_.Write(ADDR_ITG3200, 0x16, 0x18 + 4);  // enable, 20Hz bandwidth
  // config compass
  i2c_.Write(ADDR_HMC5883L, 0x00, 0x38);  // CRA: 75Hz rate w/ 2 averages
  i2c_.Write(ADDR_HMC5883L, 0x01, 0x20);  // CRB: set gain 1090 LSB/Gauss
  i2c_.Write(ADDR_HMC5883L, 0x02, 0x00);  // continuous measurement
  // config accelerometer
  i2c_.Write(ADDR_ADXL345, 0x2c, 0x09);  // 25Hz bw, 50Hz samplerate
  i2c_.Write(ADDR_ADXL345, 0x31, 0x08);  // FULL_RES
  i2c_.Write(ADDR_ADXL345, 0x38, 0x00);  // bypass FIFO, sample @ 50Hz
  i2c_.Write(ADDR_ADXL345, 0x2d, 0x08);  // turn on

  return true;
}

bool IMU::ReadRaw(IMURawState *s) {
  uint8_t axis_buf[8];
  if (!i2c_.Read(ADDR_ITG3200, 0x1b, axis_buf, 8))
    return false;
  // temperature is 280 LSB/deg C, -13200 LSB @35 C
  s->gyro_temp = bswap_16(*reinterpret_cast<uint16_t*>(axis_buf+0));
  s->gyro_x = bswap_16(*reinterpret_cast<uint16_t*>(axis_buf+2));  // roll
  s->gyro_y = bswap_16(*reinterpret_cast<uint16_t*>(axis_buf+4));  // pitch
  s->gyro_z = bswap_16(*reinterpret_cast<uint16_t*>(axis_buf+6));  // yaw

  if (!i2c_.Read(ADDR_HMC5883L, 0x03, axis_buf, 6))
    return false;
  s->mag_x = bswap_16(*reinterpret_cast<uint16_t*>(axis_buf+0));  // front?
  s->mag_z = bswap_16(*reinterpret_cast<uint16_t*>(axis_buf+2));  // up
  s->mag_y = bswap_16(*reinterpret_cast<uint16_t*>(axis_buf+4));  // side?

  if (!i2c_.Read(ADDR_ADXL345, 0x32, axis_buf, 6))
    return false;
  s->accel_x = (*reinterpret_cast<uint16_t*>(axis_buf+0));  // toward back
  s->accel_y = (*reinterpret_cast<uint16_t*>(axis_buf+2));  // toward right
  s->accel_z = (*reinterpret_cast<uint16_t*>(axis_buf+4));  // toward ground

  return true;
}

bool IMU::ReadCalibrated(IMUState *s) {
  IMURawState rawstate;
  if (!ReadRaw(&rawstate))
    return false;

  Calibrate(rawstate, s);
  return true;
}

// all devices are oriented such that X is to the front of the car, Y is to
// the left, and Z is up.
void IMU::Calibrate(const IMURawState &s, IMUState *state) {
  // gyro offset calibration (rough -- needs work)
  Vector3f w0(-s.gyro_y, -s.gyro_z, s.gyro_x);
  w0 -= gyrocal_b + s.gyro_temp * gyrocal_m;
  // scale to radians/second
  state->w = w0 * M_PI / (180.0 * 14.375);

  // magnetometer calibration via least-squares fit sphere
  // TODO: fix the axes (i think x and z are swapped and one is negated)
  Vector3f m(s.mag_x, s.mag_y, s.mag_z);
  // if there's room in the point cloud for this new point, update calibration
  if (CheckPointCloud(m, &mag_cal_points_)) {
    Vector4f b(s.mag_x, s.mag_y, s.mag_z, 1);
    mag_XTX_ += b * b.transpose();
    float bmag = b.squaredNorm();
    mag_XTY_ += bmag * b;
    Vector4f beta = mag_XTX_.ldlt().solve(mag_XTY_);
    mag_bias_ = beta.block<3, 1>(0, 0) / 2;
    bmag = sqrt(beta[3] + mag_bias_.squaredNorm());
    fprintf(stderr, "imu: updated mag calibration [%f %f %f] R=%f\n",
            beta[0] / 2, beta[1] / 2, beta[2] / 2, bmag);
  }
  state->N = m - mag_bias_;

  // TODO: verify axes
  state->g = Vector3f(-s.accel_y, -s.accel_z, -s.accel_x);
}