/**
 * @file opencv_fb.cpp
 * Displays OpenCV video on framebuffer.
 * Compile with
 * g++ -o fbwrite_video -lopencv_core -lopencv_highgui -lopencv_imgproc fbwrite_video.cpp
 *
 * Contains code from https://stackoverflow.com/questions/4722301/writing-to-frame-buffer
 */

#include <iostream> // for cerr
#include <opencv2/imgproc/imgproc.hpp>  // for cvtColor
#include <opencv2/highgui/highgui.hpp> // for VideoCapture
#include <fstream> // for ofstream
#include <boost/timer/timer.hpp> // for boost::timer::cpu_timer

using namespace std;
using namespace cv;

// this is C :/
#include <stdint.h> // for uint32_t
#include <sys/ioctl.h> // for ioctl
#include <linux/fb.h> // for fb_
#include <fcntl.h> // for O_RDWR

struct framebuffer_info { 
    uint32_t bits_per_pixel;
    uint32_t xres_virtual;
    uint32_t yres_virtual;
};

struct framebuffer_info get_framebuffer_info(const char* framebuffer_device_path) {
    struct framebuffer_info info;
    struct fb_var_screeninfo screen_info;
    int fd = -1;
    fd = open(framebuffer_device_path, O_RDWR);
    if (fd >= 0) {
        if (!ioctl(fd, FBIOGET_VSCREENINFO, &screen_info)) {
            info.xres_virtual = screen_info.xres_virtual;
            info.yres_virtual = screen_info.yres_virtual;
            info.bits_per_pixel = screen_info.bits_per_pixel;
        }
    }
    return info;
};
// C ends here

void display (Mat frame, framebuffer_info fb_info, ofstream& ofs) {
    if (frame.depth() != CV_8U) {
        cerr << "Not 8 bits per pixel and channel." << endl;
    } else if (frame.channels() != 3) {
        cerr << "Not 3 channels." << endl;
    } else {
        // 3 Channels (assumed BGR), 8 Bit per Pixel and Channel
        int framebuffer_width = fb_info.xres_virtual;
        int framebuffer_depth = fb_info.bits_per_pixel;
        Size2f frame_size = frame.size();
        Mat framebuffer_compat;
        switch (framebuffer_depth) {
            case 16:
                cvtColor(frame, framebuffer_compat, COLOR_BGR2BGR565);
                for (int y = 0; y < frame_size.height ; y++) {
                    ofs.seekp(y*framebuffer_width*2);
                    ofs.write(reinterpret_cast<char*>(framebuffer_compat.ptr(y)),frame_size.width*2);
                }
            break;
            case 32: {
                    vector<Mat> split_bgr;
                    split(frame, split_bgr);
                    split_bgr.push_back(Mat(frame_size,CV_8UC1,Scalar(255)));
                    merge(split_bgr, framebuffer_compat);
                    for (int y = 0; y < frame_size.height ; y++) {
                        ofs.seekp(y*framebuffer_width*4);
                        ofs.write(reinterpret_cast<char*>(framebuffer_compat.ptr(y)),frame_size.width*4);
                    }
                } break;
            default:
                cerr << "Unsupported depth of framebuffer." << endl;
        }
    }
}

Mat frameEffects (Mat frame) {
    Mat grey;
    Mat contrast;
    Mat blur;
    Mat flipper;
    //flip(frame, flipper, 1);
    //cvtColor(flipper, grey, COLOR_BGR2GRAY);
    //grey.convertTo(contrast, -1, 1, 0);
    //cvtColor(contrast, frame, COLOR_GRAY2BGR);
    return frame;
}

int main(int, char**) {
    const int frame_rate = 24;
    int frameCount = 0;
    framebuffer_info fb_info = get_framebuffer_info("/dev/fb0");
    VideoCapture cap(0);
    if(!cap.isOpened()) {
        cerr << "Could not open video device." << endl;
        return 1;
    } else {
        cout << "Successfully opened video device." << endl;
        cap.set(CAP_PROP_FRAME_WIDTH, 1280);
        cap.set(CAP_PROP_FRAME_HEIGHT, 720);
        cap.set(CAP_PROP_FPS, frame_rate);
        //cap.set(CAP_PROP_BUFFERSIZE,3);
        cout << cap.get(CAP_PROP_FRAME_WIDTH) << "x" << cap.get(CAP_PROP_FRAME_HEIGHT) << endl;
        ofstream ofs("/dev/fb0");
        Mat frame;
        Mat displayFrame;
        while (true) {
            cap >> frame;
            frame = frameEffects(frame);
            resize(frame, displayFrame, Size(fb_info.xres_virtual, fb_info.yres_virtual), 2, 2, INTER_CUBIC);
            display(displayFrame, fb_info, ofs);
            waitKey(42);
        }
    }
}