#include <opencv2/videoio.hpp>
#include <chrono>
#include <algorithm>
#include <numeric>
#include <vector>
#include <cstdio>

static void bench(const char* name, int device, int backend, int W, int H, int frames) {
    cv::VideoCapture cap(device, backend);
    if (!cap.isOpened()) {
        std::printf("%-12s  failed to open /dev/video%d\n", name, device);
        return;
    }
    cap.set(cv::CAP_PROP_FRAME_WIDTH,  W);
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, H);

    // Warm up
    for (int i = 0; i < 5; ++i) cap.grab();

    std::vector<double> times;
    times.reserve(frames);
    for (int i = 0; i < frames; ++i) {
        auto t0 = std::chrono::steady_clock::now();
        bool ok = cap.grab();
        double ms = std::chrono::duration<double, std::milli>(
            std::chrono::steady_clock::now() - t0).count();
        if (ok) times.push_back(ms);
    }
    cap.release();

    if (times.empty()) {
        std::printf("%-12s  no frames captured\n", name);
        return;
    }
    std::sort(times.begin(), times.end());
    double avg = std::accumulate(times.begin(), times.end(), 0.0) / times.size();
    double mn  = times.front();
    double mx  = times.back();
    double p95 = times[times.size() * 95 / 100];
    std::printf("%-12s  avg=%6.1fms  min=%5.1fms  p95=%6.1fms  max=%6.1fms  (%zu frames)\n",
        name, avg, mn, p95, mx, times.size());
}

int main(int argc, char** argv) {
    int device = argc > 1 ? std::atoi(argv[1]) : 0;
    int frames = argc > 2 ? std::atoi(argv[2]) : 60;
    int W = 1920, H = 1080;

    std::printf("Benchmarking /dev/video%d at %dx%d, %d frames each\n\n", device, W, H, frames);
    bench("V4L2",     device, cv::CAP_V4L2,      W, H, frames);
    bench("GStreamer", device, cv::CAP_GSTREAMER,  W, H, frames);
    return 0;
}