<commit_before>// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2015 Intel Corporation. All Rights Reserved.

#pragma once

#include <librealsense2/rs.hpp>

#define GLFW_INCLUDE_GLU
#include <GLFW/glfw3.h>

#include <vector>
#include <algorithm>
#include <cstring>
#include <ctype.h>
#include <memory>
#include <string>
#include <sstream>
#include <iomanip>
#include <array>
#include <chrono>
#define _USE_MATH_DEFINES
#include <cmath>
#include <map>
#include <mutex>

#ifdef _MSC_VER
#ifndef GL_CLAMP_TO_BORDER
#define GL_CLAMP_TO_BORDER  0x812D
#endif
#ifndef GL_CLAMP_TO_EDGE
#define GL_CLAMP_TO_EDGE    0x812F
#endif
#endif

namespace rs2
{
    class fps_calc
    {
    public:
        fps_calc()
            : _counter(0),
            _delta(0),
            _last_timestamp(0),
            _num_of_frames(0)
        {}

        fps_calc(const fps_calc& other)
        {
            std::lock_guard<std::mutex> lock(other._mtx);
            _counter = other._counter;
            _delta = other._delta;
            _num_of_frames = other._num_of_frames;
            _last_timestamp = other._last_timestamp;
        }
        void add_timestamp(double timestamp, unsigned long long frame_counter)
        {
            std::lock_guard<std::mutex> lock(_mtx);
            if (++_counter >= _skip_frames)
            {
                if (_last_timestamp != 0)
                {
                    _delta = timestamp - _last_timestamp;
                    _num_of_frames = frame_counter - _last_frame_counter;
                }

                _last_frame_counter = frame_counter;
                _last_timestamp = timestamp;
                _counter = 0;
            }
        }

        double get_fps() const
        {
            std::lock_guard<std::mutex> lock(_mtx);
            if (_delta == 0)
                return 0;

            return (static_cast<double>(_numerator) * _num_of_frames) / _delta;
        }

    private:
        static const int _numerator = 1000;
        static const int _skip_frames = 5;
        unsigned long long _num_of_frames;
        int _counter;
        double _delta;
        double _last_timestamp;
        unsigned long long _last_frame_counter;
        mutable std::mutex _mtx;
    };

    inline float clamp(float x, float min, float max)
    {
        return std::max(std::min(max, x), min);
    }

    inline float smoothstep(float x, float min, float max)
    {
        x = clamp((x - min) / (max - min), 0.0, 1.0);
        return x*x*(3 - 2 * x);
    }

    inline float lerp(float a, float b, float t)
    {
        return b * t + a * (1 - t);
    }

    struct plane
    {
        float a;
        float b;
        float c;
        float d;
    };
    inline bool operator==(const plane& lhs, const plane& rhs) { return lhs.a == rhs.a && lhs.b == rhs.b && lhs.c == rhs.c && lhs.d == rhs.d; }

    struct float3
    {
        float x, y, z;

        float length() const { return sqrt(x*x + y*y + z*z); }

        float3 normalize() const
        {
            return (length() > 0)? float3{ x / length(), y / length(), z / length() }:*this;
        }
    };

    inline float evaluate_plane(const plane& plane, const float3& point)
    {
        return plane.a * point.x + plane.b * point.y + plane.c * point.z + plane.d;
    }

    inline float3 operator*(const float3& a, float t)
    {
        return { a.x * t, a.y * t, a.z * t };
    }

    inline float3 operator/(const float3& a, float t)
    {
        return { a.x / t, a.y / t, a.z / t };
    }

    inline float3 operator+(const float3& a, const float3& b)
    {
        return { a.x + b.x, a.y + b.y, a.z + b.z };
    }

    inline float3 operator-(const float3& a, const float3& b)
    {
        return { a.x - b.x, a.y - b.y, a.z - b.z };
    }

    inline float3 lerp(const float3& a, const float3& b, float t)
    {
        return b * t + a * (1 - t);
    }

    struct float2
    {
        float x, y;

        float length() const { return sqrt(x*x + y*y); }

        float2 normalize() const
        {
            return { x / length(), y / length() };
        }
    };

    inline float3 lerp(const std::array<float3, 4>& rect, const float2& p)
    {
        auto v1 = lerp(rect[0], rect[1], p.x);
        auto v2 = lerp(rect[3], rect[2], p.x);
        return lerp(v1, v2, p.y);
    }

    using plane_3d = std::array<float3, 4>;

    inline std::vector<plane_3d> subdivide(const plane_3d& rect, int parts = 4)
    {
        std::vector<plane_3d> res;
        res.reserve(parts*parts);
        for (float i = 0.f; i < parts; i++)
        {
            for (float j = 0.f; j < parts; j++)
            {
                plane_3d r;
                r[0] = lerp(rect, { i / parts, j / parts });
                r[1] = lerp(rect, { i / parts, (j + 1) / parts });
                r[2] = lerp(rect, { (i + 1) / parts, (j + 1) / parts });
                r[3] = lerp(rect, { (i + 1) / parts, j / parts });
                res.push_back(r);
            }
        }
        return res;
    }

    inline float operator*(const float3& a, const float3& b)
    {
        return a.x*b.x + a.y*b.y + a.z*b.z;
    }

    inline bool is_valid(const plane_3d& p)
    {
        std::vector<float> angles;
        angles.reserve(4);
        for (int i = 0; i < p.size(); i++)
        {
            auto p1 = p[i];
            auto p2 = p[(i+1) % p.size()];
            if ((p2 - p1).length() < 1e-3) return false;

            p1 = p1.normalize();
            p2 = p2.normalize();

            angles.push_back(acos((p1 * p2) / sqrt(p1.length() * p2.length())));
        }
        return std::all_of(angles.begin(), angles.end(), [](float f) { return f > 0; }) ||
               std::all_of(angles.begin(), angles.end(), [](float f) { return f < 0; });
    }

    inline float2 operator-(float2 a, float2 b)
    {
        return { a.x - b.x, a.y - b.y };
    }

    inline float2 operator*(float a, float2 b)
    {
        return { a * b.x, a * b.y };
    }

    struct mouse_info
    {
        float2 cursor;
        float2 prev_cursor{ 0.f, 0.f };
        bool mouse_down = false;
        int mouse_wheel = 0;
        float ui_wheel = 0.f;
    };

    template<typename T>
    T normalizeT(const T& in_val, const T& min, const T& max)
    {
        return ((in_val - min)/(max - min));
    }

    template<typename T>
    T unnormalizeT(const T& in_val, const T& min, const T& max)
    {
        if (min == max) return min;
        return ((in_val * (max - min)) + min);
    }

    struct rect
    {
        float x, y;
        float w, h;

        operator bool() const
        {
            return w*w > 0 && h*h > 0;
        }

        bool operator==(const rect& other) const
        {
            return x == other.x && y == other.y && w == other.w && h == other.h;
        }

        bool operator!=(const rect& other) const
        {
            return !(*this == other);
        }

        rect normalize(const rect& normalize_to) const
        {
            return rect{normalizeT(x, normalize_to.x, normalize_to.x + normalize_to.w),
                        normalizeT(y, normalize_to.y, normalize_to.y + normalize_to.h),
                        normalizeT(w, 0.f, normalize_to.w),
                        normalizeT(h, 0.f, normalize_to.h)};
        }

        rect unnormalize(const rect& unnormalize_to) const
        {
            return rect{unnormalizeT(x, unnormalize_to.x, unnormalize_to.x + unnormalize_to.w),
                        unnormalizeT(y, unnormalize_to.y, unnormalize_to.y + unnormalize_to.h),
                        unnormalizeT(w, 0.f, unnormalize_to.w),
                        unnormalizeT(h, 0.f, unnormalize_to.h)};
        }

        // Calculate the intersection between two rects
        // If the intersection is empty, a rect with width and height zero will be returned
        rect intersection(const rect& other) const
        {
            auto x1 = std::max(x, other.x);
            auto y1 = std::max(y, other.y);
            auto x2 = std::min(x + w, other.x + other.w);
            auto y2 = std::min(y + h, other.y + other.h);

            return{
                x1, y1,
                std::max(x2 - x1, 0.f),
                std::max(y2 - y1, 0.f)
            };
        }

        // Calculate the area of the rect
        float area() const
        {
            return w * h;
        }

        rect cut_by(const rect& r) const
        {
            auto x1 = x;
            auto y1 = y;
            auto x2 = x + w;
            auto y2 = y + h;

            x1 = std::max(x1, r.x);
            x1 = std::min(x1, r.x + r.w);
            y1 = std::max(y1, r.y);
            y1 = std::min(y1, r.y + r.h);

            x2 = std::max(x2, r.x);
            x2 = std::min(x2, r.x + r.w);
            y2 = std::max(y2, r.y);
            y2 = std::min(y2, r.y + r.h);

            return { x1, y1, x2 - x1, y2 - y1 };
        }

        bool contains(const float2& p) const
        {
            return (p.x >= x) && (p.x < x + w) && (p.y >= y) && (p.y < y + h);
        }

        rect pan(const float2& p) const
        {
            return { x - p.x, y - p.y, w, h };
        }

        rect center() const
        {
            return{ x + w / 2.f, y + h / 2.f, 0, 0 };
        }

        rect lerp(float t, const rect& other) const
        {
            return{
                rs2::lerp(x, other.x, t), rs2::lerp(y, other.y, t),
                rs2::lerp(w, other.w, t), rs2::lerp(h, other.h, t),
            };
        }

        rect adjust_ratio(float2 size) const
        {
            auto H = static_cast<float>(h), W = static_cast<float>(h) * size.x / size.y;
            if (W > w)
            {
                auto scale = w / W;
                W *= scale;
                H *= scale;
            }

            return{ float(floor(x + floor(w - W) / 2)),
                    float(floor(y + floor(h - H) / 2)),
                    W, H };
        }

        rect scale(float factor) const
        {
            return { x, y, w * factor, h * factor };
        }

        rect grow(int pixels) const
        {
            return { x - pixels, y - pixels, w + pixels*2, h + pixels*2 };
        }

        rect grow(int dx, int dy) const
        {
            return { x - dx, y - dy, w + dx*2, h + dy*2 };
        }

        rect shrink_by(float2 pixels) const
        {
            return { x + pixels.x, y + pixels.y, w - pixels.x * 2, h - pixels.y * 2 };
        }

        rect center_at(const float2& new_center) const
        {
            auto c = center();
            auto diff_x = new_center.x - c.x;
            auto diff_y = new_center.y - c.y;

            return { x + diff_x, y + diff_y, w, h };
        }

        rect fit(rect r) const
        {
            float new_w = w;
            float new_h = h;

            if (w < r.w)
                new_w = r.w;

            if (h < r.h)
                new_h = r.h;

            auto res = rect{x, y, new_w, new_h};
            return res.adjust_ratio({w,h});
        }

        rect zoom(float zoom_factor) const
        {
            auto c = center();
            return scale(zoom_factor).center_at({c.x,c.y});
        }

        rect enclose_in(rect in_rect) const
        {
            rect out_rect{x, y, w, h};
            if (w > in_rect.w || h > in_rect.h)
            {
                return in_rect;
            }

            if (x < in_rect.x)
            {
                out_rect.x = in_rect.x;
            }

            if (y < in_rect.y)
            {
                out_rect.y = in_rect.y;
            }


            if (x + w > in_rect.x + in_rect.w)
            {
                out_rect.x = in_rect.x + in_rect.w - w;
            }

            if (y + h > in_rect.y + in_rect.h)
            {
                out_rect.y = in_rect.y + in_rect.h - h;
            }

            return out_rect;
        }

        bool intersects(const rect& other) const
        {
            return other.contains({ x, y }) || other.contains({ x + w, y }) ||
                other.contains({ x, y + h }) || other.contains({ x + w, y + h }) ||
                contains({ other.x, other.y });
        }
    };

    //////////////////////////////
    // Simple font loading code //
    //////////////////////////////

#include "../third-party/stb_easy_font.h"

    inline void draw_text(int x, int y, const char * text)
    {
        char buffer[60000]; // ~300 chars
        glEnableClientState(GL_VERTEX_ARRAY);
        glVertexPointer(2, GL_FLOAT, 16, buffer);
        glDrawArrays(GL_QUADS, 0, 4 * stb_easy_font_print((float)x, (float)(y - 7), (char *)text, nullptr, buffer, sizeof(buffer)));
        glDisableClientState(GL_VERTEX_ARRAY);
    }

    ////////////////////////
    // Image display code //
    ////////////////////////

    class color_map
    {
    public:
        color_map(std::map<float, float3> map, int steps = 4000) : _map(map)
        {
            initialize(steps);
        }

        color_map(const std::vector<float3>& values, int steps = 4000)
        {
            for (size_t i = 0; i < values.size(); i++)
            {
                _map[(float)i/(values.size()-1)] = values[i];
            }
            initialize(steps);
        }

        color_map() {}

        float3 get(float value) const
        {
            if (_max == _min) return *_data;
            auto t = (value - _min) / (_max - _min);
            t = std::min(std::max(t, 0.f), 1.f);
            return _data[(int)(t * (_size - 1))];
        }

        float min_key() const { return _min; }
        float max_key() const { return _max; }

    private:
        float3 calc(float value) const
        {
            if (_map.size() == 0) return { value, value, value };
            // if we have exactly this value in the map, just return it
            if( _map.find(value) != _map.end() ) return _map.at(value);
            // if we are beyond the limits, return the first/last element
            if( value < _map.begin()->first )   return _map.begin()->second;
            if( value > _map.rbegin()->first )  return _map.rbegin()->second;

            auto lower = _map.lower_bound(value) == _map.begin() ? _map.begin() : --(_map.lower_bound(value)) ;
            auto upper = _map.upper_bound(value);

            auto t = (value - lower->first) / (upper->first - lower->first);
            auto c1 = lower->second;
            auto c2 = upper->second;
            return lerp(c1, c2, t);
        }

        void initialize(int steps)
        {
            if (_map.size() == 0) return;

            _min = _map.begin()->first;
            _max = _map.rbegin()->first;

            _cache.resize(steps + 1);
            for (int i = 0; i <= steps; i++)
            {
                auto t = (float)i/steps;
                auto x = _min + t*(_max - _min);
                _cache[i] = calc(x);
            }

            // Save size and data to avoid STL checks penalties in DEBUG
            _size = _cache.size();
            _data = _cache.data();
        }

        std::map<float, float3> _map;
        std::vector<float3> _cache;
        float _min, _max;
        size_t _size; float3* _data;
    };

    static color_map classic {{
            { 255, 0, 0 },
            { 0, 0, 255 },
        }};

    static color_map jet {{
            { 0, 0, 255 },
            { 0, 255, 255 },
            { 255, 255, 0 },
            { 255, 0, 0 },
            { 50, 0, 0 },
        }};

    static color_map hsv {{
            { 255, 0, 0 },
            { 255, 255, 0 },
            { 0, 255, 0 },
            { 0, 255, 255 },
            { 0, 0, 255 },
            { 255, 0, 255 },
            { 255, 0, 0 },
        }};


    static std::vector<color_map*> color_maps { &classic, &jet, &hsv };
    static std::vector<const char*> color_maps_names { "Classic", "Jet", "HSV" };

/*    inline void make_depth_histogram(const color_map& map, uint8_t rgb_image[], const uint16_t depth_image[], int width, int height, bool equalize, float min, float max)
    {
        const auto max_depth = 0x10000;
        if (equalize)
        {
            static uint32_t histogram[max_depth];
            memset(histogram, 0, sizeof(histogram));

            for (auto i = 0; i < width*height; ++i) ++histogram[depth_image[i]];
            for (auto i = 2; i < max_depth; ++i) histogram[i] += histogram[i - 1]; // Build a cumulative histogram for the indices in [1,0xFFFF]
            for (auto i = 0; i < width*height; ++i)
            {
                auto d = depth_image[i];

                if (d)
                {
                    auto f = histogram[d] / (float)histogram[0xFFFF]; // 0-255 based on histogram location

                    auto c = map.get(f);
                    rgb_image[i * 3 + 0] = (uint8_t)c.x;
                    rgb_image[i * 3 + 1] = (uint8_t)c.y;
                    rgb_image[i * 3 + 2] = (uint8_t)c.z;
                }
                else
                {
                    rgb_image[i * 3 + 0] = 0;
                    rgb_image[i * 3 + 1] = 0;
                    rgb_image[i * 3 + 2] = 0;
                }
            }
        }
        else
        {
            for (auto i = 0; i < width*height; ++i)
            {
                auto d = depth_image[i];

                if (d)
                {
                    auto f = (d - min) / (max - min);

                    auto c = map.get(f);
                    rgb_image[i * 3 + 0] = (uint8_t)c.x;
                    rgb_image[i * 3 + 1] = (uint8_t)c.y;
                    rgb_image[i * 3 + 2] = (uint8_t)c.z;
                }
                else
                {
                    rgb_image[i * 3 + 0] = 0;
                    rgb_image[i * 3 + 1] = 0;
                    rgb_image[i * 3 + 2] = 0;
                }
            }
        }
    } */

    using clock = std::chrono::steady_clock;

    // Helper class to keep track of time
    class timer
    {
    public:
        timer()
        {
            _start = std::chrono::steady_clock::now();
        }

        void reset() { _start = std::chrono::steady_clock::now(); }

        // Get elapsed milliseconds since timer creation
        double elapsed_ms() const
        {
            return std::chrono::duration_cast<std::chrono::duration<double, std::milli>>(elapsed()).count();
        }

        clock::duration elapsed() const
        {
            return clock::now() - _start;
        }

        clock::time_point now() const
        {
            return clock::now();
        }
    private:
        clock::time_point _start;
    };

    class periodic_timer
    {
    public:
        periodic_timer(clock::duration delta)
            : _delta(delta)
        {
            _last = _time.now();
        }

        operator bool() const
        {
            if (_time.now() - _last > _delta)
            {
                _last = _time.now();
                return true;
            }
            return false;
        }

    private:
        timer _time;
        mutable clock::time_point _last;
        clock::duration _delta;
    };

    // Temporal event is a very simple time filter
    // that allows a concensus based on a set of measurements in time
    // You set the window, and add measurements, and the class offers
    // the most agreed upon opinion within the set time window
    // It is useful to remove noise from UX elements
    class temporal_event
    {
    public:
        temporal_event(clock::duration window) : _window(window) {}
        temporal_event() : _window(std::chrono::milliseconds(1000)) {}

        void add_value(bool val)
        {
            std::lock_guard<std::mutex> lock(_m);
            _measurements.push_back(std::make_pair(clock::now(), val));
        }

        bool eval()
        {
            std::lock_guard<std::mutex> lock(_m);

            if (_t.elapsed() < _window) return false; // Ensure no false alarms in the warm-up time

            _measurements.erase(std::remove_if(_measurements.begin(), _measurements.end(),
                [this](std::pair<clock::time_point, bool> pair) {
                return (clock::now() - pair.first) > _window;
            }),
                _measurements.end());
            auto trues = std::count_if(_measurements.begin(), _measurements.end(),
                [this](std::pair<clock::time_point, bool> pair) {
                return pair.second;
            });
            return size_t(trues * 2) > _measurements.size(); // At least 50% of observations agree
        }

        void reset()
        {
            std::lock_guard<std::mutex> lock(_m);
            _t.reset();
            _measurements.clear();
        }

    private:
        std::mutex _m;
        clock::duration _window;
        std::vector<std::pair<clock::time_point, bool>> _measurements;
        timer _t;
    };

    class texture_buffer
    {
        GLuint texture;
        rs2::frame_queue last_queue[2];
        mutable rs2::frame last[2];
    public:
        std::shared_ptr<colorizer> colorize;

        texture_buffer(const texture_buffer& other)
        {
            texture = other.texture;
        }

        texture_buffer& operator =(const texture_buffer& other)
        {
            texture = other.texture;
        }

        rs2::frame get_last_frame(bool with_texture = false) const {
            auto idx = with_texture ? 1 : 0;
            last_queue[idx].poll_for_frame(&last[idx]);
            return last[idx];
        }

        texture_buffer() : last_queue(), texture(),
            colorize(std::make_shared<colorizer>()) {}

        GLuint get_gl_handle() const { return texture; }

        // Simplified version of upload that lets us load basic RGBA textures
        // This is used for the splash screen
        void upload_image(int w, int h, void* data)
        {
            if (!texture)
                glGenTextures(1, &texture);
            glBindTexture(GL_TEXTURE_2D, texture);
            glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, data);
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
            glBindTexture(GL_TEXTURE_2D, 0);
        }

        void upload(rs2::frame frame)
        {
            last_queue[0].enqueue(frame);
            // If the frame timestamp has changed since the last time show(...) was called, re-upload the texture
            if (!texture)
                glGenTextures(1, &texture);

            int width = 0;
            int height = 0;
            int stride = 0;
            auto format = frame.get_profile().format();
            auto data = frame.get_data();

            auto image = frame.as<video_frame>();
            auto rendered_frame = image;

            if (image)
            {
                width = image.get_width();
                height = image.get_height();
                stride = image.get_stride_in_bytes();
            }

            glBindTexture(GL_TEXTURE_2D, texture);
            stride = stride == 0 ? width : stride;
            //glPixelStorei(GL_UNPACK_ROW_LENGTH, stride);
            switch (format)
            {
            case RS2_FORMAT_ANY:
                throw std::runtime_error("not a valid format");
            case RS2_FORMAT_Z16:
            case RS2_FORMAT_DISPARITY16:
                if (frame.is<depth_frame>())
                {
                    if (rendered_frame = colorize->colorize(frame).as<video_frame>())
                    {
                        data = rendered_frame.get_data();
                        glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB,
                                     rendered_frame.get_width(),
                                     rendered_frame.get_height(),
                                     0, GL_RGB, GL_UNSIGNED_BYTE,
                                     rendered_frame.get_data());
                    }
                }
                else glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_LUMINANCE, GL_UNSIGNED_SHORT, data);

                break;
            case RS2_FORMAT_DISPARITY32:
                glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT, width, height, 0, GL_DEPTH_COMPONENT, GL_FLOAT, data);
                break;
            case RS2_FORMAT_XYZ32F:
                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_FLOAT, data);
                break;
            case RS2_FORMAT_YUYV: // Display YUYV by showing the luminance channel and packing chrominance into ignored alpha channel
                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_LUMINANCE_ALPHA, GL_UNSIGNED_BYTE, data);
                break;
            case RS2_FORMAT_UYVY: // Use one color component only to avoid costly UVUY->RGB conversion
                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_GREEN, GL_UNSIGNED_SHORT, data);
                break;
            case RS2_FORMAT_RGB8: case RS2_FORMAT_BGR8: // Display both RGB and BGR by interpreting them RGB, to show the flipped byte ordering. Obviously, GL_BGR could be used on OpenGL 1.2+
                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, data);
                break;
            case RS2_FORMAT_RGBA8: case RS2_FORMAT_BGRA8: // Display both RGBA and BGRA by interpreting them RGBA, to show the flipped byte ordering. Obviously, GL_BGRA could be used on OpenGL 1.2+
                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, data);
                break;
            case RS2_FORMAT_Y8:
                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, data);
                break;
            case RS2_FORMAT_MOTION_XYZ32F:
            {
                auto axes = *(reinterpret_cast<const float3*>(data));
                draw_motion_data(axes.x, axes.y, axes.z);
            }
            break;
            case RS2_FORMAT_Y16:
                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_LUMINANCE, GL_UNSIGNED_SHORT, data);
                break;
            case RS2_FORMAT_RAW8:
            case RS2_FORMAT_MOTION_RAW:
            case RS2_FORMAT_GPIO_RAW:
                glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE, width, height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, data);
                break;
            //case RS2_FORMAT_RAW10:
            //{
            //    // Visualize Raw10 by performing a naive downsample. Each 2x2 block contains one red pixel, two green pixels, and one blue pixel, so combine them into a single RGB triple.
            //    rgb.clear(); rgb.resize(width / 2 * height / 2 * 3);
            //    auto out = rgb.data(); auto in0 = reinterpret_cast<const uint8_t *>(data), in1 = in0 + width * 5 / 4;
            //    for (auto y = 0; y<height; y += 2)
            //    {
            //        for (auto x = 0; x<width; x += 4)
            //        {
            //            *out++ = in0[0]; *out++ = (in0[1] + in1[0]) / 2; *out++ = in1[1]; // RGRG -> RGB RGB
            //            *out++ = in0[2]; *out++ = (in0[3] + in1[2]) / 2; *out++ = in1[3]; // GBGB
            //            in0 += 5; in1 += 5;
            //        }
            //        in0 = in1; in1 += width * 5 / 4;
            //    }
            //    glPixelStorei(GL_UNPACK_ROW_LENGTH, width / 2);        // Update row stride to reflect post-downsampling dimensions of the target texture
            //    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width / 2, height / 2, 0, GL_RGB, GL_UNSIGNED_BYTE, rgb.data());
            //}
            //break;
            default:
                throw std::runtime_error("The requested format is not supported for rendering");
            }
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
            glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
            glBindTexture(GL_TEXTURE_2D, 0);

            last_queue[1].enqueue(rendered_frame);
        }

        static void draw_axis(float axis_size = 1.f, float axisWidth = 4.f)
        {

            // Traingles For X axis
            glBegin(GL_TRIANGLES);
            glColor3f(1.0f, 0.0f, 0.0f);
            glVertex3f(axis_size * 1.1f, 0.0f, 0.0f);
            glVertex3f(axis_size, axis_size * 0.05f, 0.0f);
            glVertex3f(axis_size, -0.05f * axis_size, 0.0f);
            glEnd();<commit_msg>Fix OpenGl alignment when rendering arbitrary-stride images<commit_after>// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2015 Intel Corporation. All Rights Reserved.

#pragma once

#include <librealsense2/rs.hpp>

#define GLFW_INCLUDE_GLU
#include <GLFW/glfw3.h>

#include <vector>
#include <algorithm>
#include <cstring>
#include <ctype.h>
#include <memory>
#include <string>
#include <sstream>
#include <iomanip>
#include <array>
#include <chrono>
#define _USE_MATH_DEFINES
#include <cmath>
#include <map>
#include <mutex>

#ifdef _MSC_VER
#ifndef GL_CLAMP_TO_BORDER
#define GL_CLAMP_TO_BORDER  0x812D
#endif
#ifndef GL_CLAMP_TO_EDGE
#define GL_CLAMP_TO_EDGE    0x812F
#endif
#endif

namespace rs2
{
    class fps_calc
    {
    public:
        fps_calc()
            : _counter(0),
            _delta(0),
            _last_timestamp(0),
            _num_of_frames(0)
        {}

        fps_calc(const fps_calc& other)
        {
            std::lock_guard<std::mutex> lock(other._mtx);
            _counter = other._counter;
            _delta = other._delta;
            _num_of_frames = other._num_of_frames;
            _last_timestamp = other._last_timestamp;
        }
        void add_timestamp(double timestamp, unsigned long long frame_counter)
        {
            std::lock_guard<std::mutex> lock(_mtx);
            if (++_counter >= _skip_frames)
            {
                if (_last_timestamp != 0)
                {
                    _delta = timestamp - _last_timestamp;
                    _num_of_frames = frame_counter - _last_frame_counter;
                }

                _last_frame_counter = frame_counter;
                _last_timestamp = timestamp;
                _counter = 0;
            }
        }

        double get_fps() const
        {
            std::lock_guard<std::mutex> lock(_mtx);
            if (_delta == 0)
                return 0;

            return (static_cast<double>(_numerator) * _num_of_frames) / _delta;
        }

    private:
        static const int _numerator = 1000;
        static const int _skip_frames = 5;
        unsigned long long _num_of_frames;
        int _counter;
        double _delta;
        double _last_timestamp;
        unsigned long long _last_frame_counter;
        mutable std::mutex _mtx;
    };

    inline float clamp(float x, float min, float max)
    {
        return std::max(std::min(max, x), min);
    }

    inline float smoothstep(float x, float min, float max)
    {
        x = clamp((x - min) / (max - min), 0.0, 1.0);
        return x*x*(3 - 2 * x);
    }

    inline float lerp(float a, float b, float t)
    {
        return b * t + a * (1 - t);
    }

    struct plane
    {
        float a;
        float b;
        float c;
        float d;
    };
    inline bool operator==(const plane& lhs, const plane& rhs) { return lhs.a == rhs.a && lhs.b == rhs.b && lhs.c == rhs.c && lhs.d == rhs.d; }

    struct float3
    {
        float x, y, z;

        float length() const { return sqrt(x*x + y*y + z*z); }

        float3 normalize() const
        {
            return (length() > 0)? float3{ x / length(), y / length(), z / length() }:*this;
        }
    };

    inline float evaluate_plane(const plane& plane, const float3& point)
    {
        return plane.a * point.x + plane.b * point.y + plane.c * point.z + plane.d;
    }

    inline float3 operator*(const float3& a, float t)
    {
        return { a.x * t, a.y * t, a.z * t };
    }

    inline float3 operator/(const float3& a, float t)
    {
        return { a.x / t, a.y / t, a.z / t };
    }

    inline float3 operator+(const float3& a, const float3& b)
    {
        return { a.x + b.x, a.y + b.y, a.z + b.z };
    }

    inline float3 operator-(const float3& a, const float3& b)
    {
        return { a.x - b.x, a.y - b.y, a.z - b.z };
    }

    inline float3 lerp(const float3& a, const float3& b, float t)
    {
        return b * t + a * (1 - t);
    }

    struct float2
    {
        float x, y;

        float length() const { return sqrt(x*x + y*y); }

        float2 normalize() const
        {
            return { x / length(), y / length() };
        }
    };

    inline float3 lerp(const std::array<float3, 4>& rect, const float2& p)
    {
        auto v1 = lerp(rect[0], rect[1], p.x);
        auto v2 = lerp(rect[3], rect[2], p.x);
        return lerp(v1, v2, p.y);
    }

    using plane_3d = std::array<float3, 4>;

    inline std::vector<plane_3d> subdivide(const plane_3d& rect, int parts = 4)
    {
        std::vector<plane_3d> res;
        res.reserve(parts*parts);
        for (float i = 0.f; i < parts; i++)
        {
            for (float j = 0.f; j < parts; j++)
            {
                plane_3d r;
                r[0] = lerp(rect, { i / parts, j / parts });
                r[1] = lerp(rect, { i / parts, (j + 1) / parts });
                r[2] = lerp(rect, { (i + 1) / parts, (j + 1) / parts });
                r[3] = lerp(rect, { (i + 1) / parts, j / parts });
                res.push_back(r);
            }
        }
        return res;
    }

    inline float operator*(const float3& a, const float3& b)
    {
        return a.x*b.x + a.y*b.y + a.z*b.z;
    }

    inline bool is_valid(const plane_3d& p)
    {
        std::vector<float> angles;
        angles.reserve(4);
        for (int i = 0; i < p.size(); i++)
        {
            auto p1 = p[i];
            auto p2 = p[(i+1) % p.size()];
            if ((p2 - p1).length() < 1e-3) return false;

            p1 = p1.normalize();
            p2 = p2.normalize();

            angles.push_back(acos((p1 * p2) / sqrt(p1.length() * p2.length())));
        }
        return std::all_of(angles.begin(), angles.end(), [](float f) { return f > 0; }) ||
               std::all_of(angles.begin(), angles.end(), [](float f) { return f < 0; });
    }

    inline float2 operator-(float2 a, float2 b)
    {
        return { a.x - b.x, a.y - b.y };
    }

    inline float2 operator*(float a, float2 b)
    {
        return { a * b.x, a * b.y };
    }

    struct mouse_info
    {
        float2 cursor;
        float2 prev_cursor{ 0.f, 0.f };
        bool mouse_down = false;
        int mouse_wheel = 0;
        float ui_wheel = 0.f;
    };

    template<typename T>
    T normalizeT(const T& in_val, const T& min, const T& max)
    {
        return ((in_val - min)/(max - min));
    }

    template<typename T>
    T unnormalizeT(const T& in_val, const T& min, const T& max)
    {
        if (min == max) return min;
        return ((in_val * (max - min)) + min);
    }

    struct rect
    {
        float x, y;
        float w, h;

        operator bool() const
        {
            return w*w > 0 && h*h > 0;
        }

        bool operator==(const rect& other) const
        {
            return x == other.x && y == other.y && w == other.w && h == other.h;
        }

        bool operator!=(const rect& other) const
        {
            return !(*this == other);
        }

        rect normalize(const rect& normalize_to) const
        {
            return rect{normalizeT(x, normalize_to.x, normalize_to.x + normalize_to.w),
                        normalizeT(y, normalize_to.y, normalize_to.y + normalize_to.h),
                        normalizeT(w, 0.f, normalize_to.w),
                        normalizeT(h, 0.f, normalize_to.h)};
        }

        rect unnormalize(const rect& unnormalize_to) const
        {
            return rect{unnormalizeT(x, unnormalize_to.x, unnormalize_to.x + unnormalize_to.w),
                        unnormalizeT(y, unnormalize_to.y, unnormalize_to.y + unnormalize_to.h),
                        unnormalizeT(w, 0.f, unnormalize_to.w),
                        unnormalizeT(h, 0.f, unnormalize_to.h)};
        }

        // Calculate the intersection between two rects
        // If the intersection is empty, a rect with width and height zero will be returned
        rect intersection(const rect& other) const
        {
            auto x1 = std::max(x, other.x);
            auto y1 = std::max(y, other.y);
            auto x2 = std::min(x + w, other.x + other.w);
            auto y2 = std::min(y + h, other.y + other.h);

            return{
                x1, y1,
                std::max(x2 - x1, 0.f),
                std::max(y2 - y1, 0.f)
            };
        }

        // Calculate the area of the rect
        float area() const
        {
            return w * h;
        }

        rect cut_by(const rect& r) const
        {
            auto x1 = x;
            auto y1 = y;
            auto x2 = x + w;
            auto y2 = y + h;

            x1 = std::max(x1, r.x);
            x1 = std::min(x1, r.x + r.w);
            y1 = std::max(y1, r.y);
            y1 = std::min(y1, r.y + r.h);

            x2 = std::max(x2, r.x);
            x2 = std::min(x2, r.x + r.w);
            y2 = std::max(y2, r.y);
            y2 = std::min(y2, r.y + r.h);

            return { x1, y1, x2 - x1, y2 - y1 };
        }

        bool contains(const float2& p) const
        {
            return (p.x >= x) && (p.x < x + w) && (p.y >= y) && (p.y < y + h);
        }

        rect pan(const float2& p) const
        {
            return { x - p.x, y - p.y, w, h };
        }

        rect center() const
        {
            return{ x + w / 2.f, y + h / 2.f, 0, 0 };
        }

        rect lerp(float t, const rect& other) const
        {
            return{
                rs2::lerp(x, other.x, t), rs2::lerp(y, other.y, t),
                rs2::lerp(w, other.w, t), rs2::lerp(h, other.h, t),
            };
        }

        rect adjust_ratio(float2 size) const
        {
            auto H = static_cast<float>(h), W = static_cast<float>(h) * size.x / size.y;
            if (W > w)
            {
                auto scale = w / W;
                W *= scale;
                H *= scale;
            }

            return{ float(floor(x + floor(w - W) / 2)),
                    float(floor(y + floor(h - H) / 2)),
                    W, H };
        }

        rect scale(float factor) const
        {
            return { x, y, w * factor, h * factor };
        }

        rect grow(int pixels) const
        {
            return { x - pixels, y - pixels, w + pixels*2, h + pixels*2 };
        }

        rect grow(int dx, int dy) const
        {
            return { x - dx, y - dy, w + dx*2, h + dy*2 };
        }

        rect shrink_by(float2 pixels) const
        {
            return { x + pixels.x, y + pixels.y, w - pixels.x * 2, h - pixels.y * 2 };
        }

        rect center_at(const float2& new_center) const
        {
            auto c = center();
            auto diff_x = new_center.x - c.x;
            auto diff_y = new_center.y - c.y;

            return { x + diff_x, y + diff_y, w, h };
        }

        rect fit(rect r) const
        {
            float new_w = w;
            float new_h = h;

            if (w < r.w)
                new_w = r.w;

            if (h < r.h)
                new_h = r.h;

            auto res = rect{x, y, new_w, new_h};
            return res.adjust_ratio({w,h});
        }

        rect zoom(float zoom_factor) const
        {
            auto c = center();
            return scale(zoom_factor).center_at({c.x,c.y});
        }

        rect enclose_in(rect in_rect) const
        {
            rect out_rect{x, y, w, h};
            if (w > in_rect.w || h > in_rect.h)
            {
                return in_rect;
            }

            if (x < in_rect.x)
            {
                out_rect.x = in_rect.x;
            }

            if (y < in_rect.y)
            {
                out_rect.y = in_rect.y;
            }


            if (x + w > in_rect.x + in_rect.w)
            {
                out_rect.x = in_rect.x + in_rect.w - w;
            }

            if (y + h > in_rect.y + in_rect.h)
            {
                out_rect.y = in_rect.y + in_rect.h - h;
            }

            return out_rect;
        }

        bool intersects(const rect& other) const
        {
            return other.contains({ x, y }) || other.contains({ x + w, y }) ||
                other.contains({ x, y + h }) || other.contains({ x + w, y + h }) ||
                contains({ other.x, other.y });
        }
    };

    //////////////////////////////
    // Simple font loading code //
    //////////////////////////////

#include "../third-party/stb_easy_font.h"

    inline void draw_text(int x, int y, const char * text)
    {
        char buffer[60000]; // ~300 chars
        glEnableClientState(GL_VERTEX_ARRAY);
        glVertexPointer(2, GL_FLOAT, 16, buffer);
        glDrawArrays(GL_QUADS, 0, 4 * stb_easy_font_print((float)x, (float)(y - 7), (char *)text, nullptr, buffer, sizeof(buffer)));
        glDisableClientState(GL_VERTEX_ARRAY);
    }

    ////////////////////////
    // Image display code //
    ////////////////////////

    class color_map
    {
    public:
        color_map(std::map<float, float3> map, int steps = 4000) : _map(map)
        {
            initialize(steps);
        }

        color_map(const std::vector<float3>& values, int steps = 4000)
        {
            for (size_t i = 0; i < values.size(); i++)
            {
                _map[(float)i/(values.size()-1)] = values[i];
            }
            initialize(steps);
        }

        color_map() {}

        float3 get(float value) const
        {
            if (_max == _min) return *_data;
            auto t = (value - _min) / (_max - _min);
            t = std::min(std::max(t, 0.f), 1.f);
            return _data[(int)(t * (_size - 1))];
        }

        float min_key() const { return _min; }
        float max_key() const { return _max; }

    private:
        float3 calc(float value) const
        {
            if (_map.size() == 0) return { value, value, value };
            // if we have exactly this value in the map, just return it
            if( _map.find(value) != _map.end() ) return _map.at(value);
            // if we are beyond the limits, return the first/last element
            if( value < _map.begin()->first )   return _map.begin()->second;
            if( value > _map.rbegin()->first )  return _map.rbegin()->second;

            auto lower = _map.lower_bound(value) == _map.begin() ? _map.begin() : --(_map.lower_bound(value)) ;
            auto upper = _map.upper_bound(value);

            auto t = (value - lower->first) / (upper->first - lower->first);
            auto c1 = lower->second;
            auto c2 = upper->second;
            return lerp(c1, c2, t);
        }

        void initialize(int steps)
        {
            if (_map.size() == 0) return;

            _min = _map.begin()->first;
            _max = _map.rbegin()->first;

            _cache.resize(steps + 1);
            for (int i = 0; i <= steps; i++)
            {
                auto t = (float)i/steps;
                auto x = _min + t*(_max - _min);
                _cache[i] = calc(x);
            }

            // Save size and data to avoid STL checks penalties in DEBUG
            _size = _cache.size();
            _data = _cache.data();
        }

        std::map<float, float3> _map;
        std::vector<float3> _cache;
        float _min, _max;
        size_t _size; float3* _data;
    };

    static color_map classic {{
            { 255, 0, 0 },
            { 0, 0, 255 },
        }};

    static color_map jet {{
            { 0, 0, 255 },
            { 0, 255, 255 },
            { 255, 255, 0 },
            { 255, 0, 0 },
            { 50, 0, 0 },
        }};

    static color_map hsv {{
            { 255, 0, 0 },
            { 255, 255, 0 },
            { 0, 255, 0 },
            { 0, 255, 255 },
            { 0, 0, 255 },
            { 255, 0, 255 },
            { 255, 0, 0 },
        }};


    static std::vector<color_map*> color_maps { &classic, &jet, &hsv };
    static std::vector<const char*> color_maps_names { "Classic", "Jet", "HSV" };

/*    inline void make_depth_histogram(const color_map& map, uint8_t rgb_image[], const uint16_t depth_image[], int width, int height, bool equalize, float min, float max)
    {
        const auto max_depth = 0x10000;
        if (equalize)
        {
            static uint32_t histogram[max_depth];
            memset(histogram, 0, sizeof(histogram));

            for (auto i = 0; i < width*height; ++i) ++histogram[depth_image[i]];
            for (auto i = 2; i < max_depth; ++i) histogram[i] += histogram[i - 1]; // Build a cumulative histogram for the indices in [1,0xFFFF]
            for (auto i = 0; i < width*height; ++i)
            {
                auto d = depth_image[i];

                if (d)
                {
                    auto f = histogram[d] / (float)histogram[0xFFFF]; // 0-255 based on histogram location

                    auto c = map.get(f);
                    rgb_image[i * 3 + 0] = (uint8_t)c.x;
                    rgb_image[i * 3 + 1] = (uint8_t)c.y;
                    rgb_image[i * 3 + 2] = (uint8_t)c.z;
                }
                else
                {
                    rgb_image[i * 3 + 0] = 0;
                    rgb_image[i * 3 + 1] = 0;
                    rgb_image[i * 3 + 2] = 0;
                }
            }
        }
        else
        {
            for (auto i = 0; i < width*height; ++i)
            {
                auto d = depth_image[i];

                if (d)
                {
                    auto f = (d - min) / (max - min);

                    auto c = map.get(f);
                    rgb_image[i * 3 + 0] = (uint8_t)c.x;
                    rgb_image[i * 3 + 1] = (uint8_t)c.y;
                    rgb_image[i * 3 + 2] = (uint8_t)c.z;
                }
                else
                {
                    rgb_image[i * 3 + 0] = 0;
                    rgb_image[i * 3 + 1] = 0;
                    rgb_image[i * 3 + 2] = 0;
                }
            }
        }
    } */

    using clock = std::chrono::steady_clock;

    // Helper class to keep track of time
    class timer
    {
    public:
        timer()
        {
            _start = std::chrono::steady_clock::now();
        }

        void reset() { _start = std::chrono::steady_clock::now(); }

        // Get elapsed milliseconds since timer creation
        double elapsed_ms() const
        {
            return std::chrono::duration_cast<std::chrono::duration<double, std::milli>>(elapsed()).count();
        }

        clock::duration elapsed() const
        {
            return clock::now() - _start;
        }

        clock::time_point now() const
        {
            return clock::now();
        }
    private:
        clock::time_point _start;
    };

    class periodic_timer
    {
    public:
        periodic_timer(clock::duration delta)
            : _delta(delta)
        {
            _last = _time.now();
        }

        operator bool() const
        {
            if (_time.now() - _last > _delta)
            {
                _last = _time.now();
                return true;
            }
            return false;
        }

    private:
        timer _time;
        mutable clock::time_point _last;
        clock::duration _delta;
    };

    // Temporal event is a very simple time filter
    // that allows a concensus based on a set of measurements in time
    // You set the window, and add measurements, and the class offers
    // the most agreed upon opinion within the set time window
    // It is useful to remove noise from UX elements
    class temporal_event
    {
    public:
        temporal_event(clock::duration window) : _window(window) {}
        temporal_event() : _window(std::chrono::milliseconds(1000)) {}

        void add_value(bool val)
        {
            std::lock_guard<std::mutex> lock(_m);
            _measurements.push_back(std::make_pair(clock::now(), val));
        }

        bool eval()
        {
            std::lock_guard<std::mutex> lock(_m);

            if (_t.elapsed() < _window) return false; // Ensure no false alarms in the warm-up time

            _measurements.erase(std::remove_if(_measurements.begin(), _measurements.end(),
                [this](std::pair<clock::time_point, bool> pair) {
                return (clock::now() - pair.first) > _window;
            }),
                _measurements.end());
            auto trues = std::count_if(_measurements.begin(), _measurements.end(),
                [this](std::pair<clock::time_point, bool> pair) {
                return pair.second;
            });
            return size_t(trues * 2) > _measurements.size(); // At least 50% of observations agree
        }

        void reset()
        {
            std::lock_guard<std::mutex> lock(_m);
            _t.reset();
            _measurements.clear();
        }

    private:
        std::mutex _m;
        clock::duration _window;
        std::vector<std::pair<clock::time_point, bool>> _measurements;
        timer _t;
    };

    class texture_buffer
    {
        GLuint texture;
        rs2::frame_queue last_queue[2];
        mutable rs2::frame last[2];
    public:
        std::shared_ptr<colorizer> colorize;

        texture_buffer(const texture_buffer& other)
        {
            texture = other.texture;
        }

        texture_buffer& operator =(const texture_buffer& other)
        {
            texture = other.texture;
        }

        rs2::frame get_last_frame(bool with_texture = false) const {
            auto idx = with_texture ? 1 : 0;
            last_queue[idx].poll_for_frame(&last[idx]);
            return last[idx];
        }

        texture_buffer() : last_queue(), texture(),
            colorize(std::make_shared<colorizer>()) {}

        GLuint get_gl_handle() const { return texture; }

        // Simplified version of upload that lets us load basic RGBA textures
        // This is used for the splash screen
        void upload_image(int w, int h, void* data)
        {
            if (!texture)
                glGenTextures(1, &texture);
            glBindTexture(GL_TEXTURE_2D, texture);
            glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, data);
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
            glBindTexture(GL_TEXTURE_2D, 0);
        }

        void upload(rs2::frame frame)
        {
            last_queue[0].enqueue(frame);
            // If the frame timestamp has changed since the last time show(...) was called, re-upload the texture
            if (!texture)
                glGenTextures(1, &texture);

            int width = 0;
            int height = 0;
            int stride = 0;
            auto format = frame.get_profile().format();
            auto data = frame.get_data();

            auto image = frame.as<video_frame>();
            auto rendered_frame = image;

            if (image)
            {
                width = image.get_width();
                height = image.get_height();
                stride = image.get_stride_in_bytes();
            }

            glBindTexture(GL_TEXTURE_2D, texture);
            stride = stride == 0 ? width : stride;
            //glPixelStorei(GL_UNPACK_ROW_LENGTH, stride);
            switch (format)
            {
            case RS2_FORMAT_ANY:
                throw std::runtime_error("not a valid format");
            case RS2_FORMAT_Z16:
            case RS2_FORMAT_DISPARITY16:
                if (frame.is<depth_frame>())
                {
                    if (rendered_frame = colorize->colorize(frame).as<video_frame>())
                    {
                        data = rendered_frame.get_data();
                        glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB,
                                     rendered_frame.get_width(),
                                     rendered_frame.get_height(),
                                     0, GL_RGB, GL_UNSIGNED_BYTE,
                                     rendered_frame.get_data());
                    }
                }
                else glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_LUMINANCE, GL_UNSIGNED_SHORT, data);

                break;
            case RS2_FORMAT_DISPARITY32:
                glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT, width, height, 0, GL_DEPTH_COMPONENT, GL_FLOAT, data);
                break;
            case RS2_FORMAT_XYZ32F:
                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_FLOAT, data);
                break;
            case RS2_FORMAT_YUYV: // Display YUYV by showing the luminance channel and packing chrominance into ignored alpha channel
                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_LUMINANCE_ALPHA, GL_UNSIGNED_BYTE, data);
                break;
            case RS2_FORMAT_UYVY: // Use one color component only to avoid costly UVUY->RGB conversion
                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_GREEN, GL_UNSIGNED_SHORT, data);
                break;
            case RS2_FORMAT_RGB8: case RS2_FORMAT_BGR8: // Display both RGB and BGR by interpreting them RGB, to show the flipped byte ordering. Obviously, GL_BGR could be used on OpenGL 1.2+
                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, data);
                break;
            case RS2_FORMAT_RGBA8: case RS2_FORMAT_BGRA8: // Display both RGBA and BGRA by interpreting them RGBA, to show the flipped byte ordering. Obviously, GL_BGRA could be used on OpenGL 1.2+
                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, data);
                break;
            case RS2_FORMAT_Y8:
                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, data);
                break;
            case RS2_FORMAT_MOTION_XYZ32F:
            {
                auto axes = *(reinterpret_cast<const float3*>(data));
                draw_motion_data(axes.x, axes.y, axes.z);
            }
            break;
            case RS2_FORMAT_Y16:
                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_LUMINANCE, GL_UNSIGNED_SHORT, data);
                break;
            case RS2_FORMAT_RAW8:
            case RS2_FORMAT_MOTION_RAW:
            case RS2_FORMAT_GPIO_RAW:
                glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE, width, height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, data);
                break;
            //case RS2_FORMAT_RAW10:
            //{
            //    // Visualize Raw10 by performing a naive downsample. Each 2x2 block contains one red pixel, two green pixels, and one blue pixel, so combine them into a single RGB triple.
            //    rgb.clear(); rgb.resize(width / 2 * height / 2 * 3);
            //    auto out = rgb.data(); auto in0 = reinterpret_cast<const uint8_t *>(data), in1 = in0 + width * 5 / 4;
            //    for (auto y = 0; y<height; y += 2)
            //    {
            //        for (auto x = 0; x<width; x += 4)
            //        {
            //            *out++ = in0[0]; *out++ = (in0[1] + in1[0]) / 2; *out++ = in1[1]; // RGRG -> RGB RGB
            //            *out++ = in0[2]; *out++ = (in0[3] + in1[2]) / 2; *out++ = in1[3]; // GBGB
            //            in0 += 5; in1 += 5;
            //        }
            //        in0 = in1; in1 += width * 5 / 4;
            //    }
            //    glPixelStorei(GL_UNPACK_ROW_LENGTH, width / 2);        // Update row stride to reflect post-downsampling dimensions of the target texture
            //    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width / 2, height / 2, 0, GL_RGB, GL_UNSIGNED_BYTE, rgb.data());
            //}
            //break;
            default:
                throw std::runtime_error("The requested format is not supported for rendering");
            }
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
            glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
            glPixelStorei(GL_PACK_ALIGNMENT, 1);
            glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
            glBindTexture(GL_TEXTURE_2D, 0);

            last_queue[1].enqueue(rendered_frame);
        }

        static void draw_axis(float axis_size = 1.f, float axisWidth = 4.f)
        {

            // Traingles For X axis
            glBegin(GL_TRIANGLES);
            glColor3f(1.0f, 0.0f, 0.0f);
            glVertex3f(axis_size * 1.1f, 0.0f, 0.0f);
            glVertex3f(axis_size, axis_size * 0.05f, 0.0f);
            glVertex3f(axis_size, -0.05f * axis_size, 0.0f);
            glEnd();
{'fraction_non_alphanumeric': 0.1244591366956281, 'fraction_numerical': 0.019938384852365952, 'mean_word_length': 1.468766022902068, 'pattern_counts': {'":': 0, '<': 145, '<?xml version=': 0, '>': 163, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 8, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5172092', 'n_tokens_mistral': 19501, 'n_tokens_neox': 18098, 'n_words': 4601}