[CPP] ArduCAM

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <jsoncpp/json/json.h>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <ctime>
#include <thread>

class FPSCounter {
public:
    FPSCounter() : frame_count(0), start_time(std::chrono::steady_clock::now()) {}

    void displayFPS(const cv::Mat& frame) {
        frame_count++;
        auto current_time = std::chrono::steady_clock::now();
        std::chrono::duration<double> elapsed = current_time - start_time;

        if (elapsed.count() >= 1.0) {
            std::cout << "Width: " << frame.cols 
                      << " Height: " << frame.rows 
                      << " FPS: " << frame_count << std::endl;
            frame_count = 0;
            start_time = current_time;
        }
    }

private:
    int frame_count;
    std::chrono::steady_clock::time_point start_time;
};


class Camera {
public:
    Camera(int index = 0, int selector = cv::CAP_V4L2)
        : index(index), selector(selector), width(0), height(0), fps(0) {}

    cv::VideoCapture cap;


    void open() {
        cap.open(index, selector);
        cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
        cap.set(cv::CAP_PROP_FPS, 60);

        if (width > 0 && height > 0) {
            set(cv::CAP_PROP_FRAME_WIDTH, width);
            set(cv::CAP_PROP_FRAME_HEIGHT, height);
        }

        if (fps > 0) {
            set(cv::CAP_PROP_FPS, fps);
        }
    }

    void setWidth(int w) { width = w; }
    void setHeight(int h) { height = h; }
    void setFPS(int f) { fps = f; }

    void setFocus(int val) { set(cv::CAP_PROP_FOCUS, val); }

    void set(int property, double value) {
        if (cap.isOpened()) {
            cap.set(property, value);
        } else {
            std::cerr << "Camera is not opened!" << std::endl;
        }
    }

    bool read(cv::Mat& frame) { return cap.read(frame); }

    void restart() {
        release();
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        open();
    }

    void release() { cap.release(); }

    bool isOpened() const { return cap.isOpened(); }
    

private:
    int index;
    int selector;
    int width;
    int height;
    int fps;
};




int main(int argc, char** argv) {

    ros::init(argc, argv, "arducam_publisher");
    ros::NodeHandle nh;
    ros::Publisher image_pub = nh.advertise<sensor_msgs::Image>("/camera/image_raw", 1000);
    cv_bridge::CvImage bridge;

    int width = 640, height = 480, fps = 50, index = 0;
    std::string output_path;
    int restart_times = 5;

    Camera camera(index);
    camera.setWidth(width);
    camera.setHeight(height);
    camera.setFPS(fps);
    camera.open();

    if (!camera.isOpened()) {
        std::cerr << "Can't open camera" << std::endl;
        return -1;
    }

    cv::namedWindow("video", cv::WINDOW_NORMAL);
    cv::resizeWindow("video", 800, 600);

    FPSCounter fps_counter;
    cv::Mat frame;

    while (ros::ok()) {
        if (!camera.read(frame)) {
            std::cerr << "Unable to read video frame" << std::endl;
            bool success = false;
            for (int i = 1; i <= restart_times; ++i) {
                std::cerr << "Reopening camera (" << i << " times)" << std::endl;
                camera.cap.open(index);
                if (camera.isOpened()) {
                    success = true;
                    break;
                }
            }
            if (!success) {
                std::cerr << "Failed to reopen camera" << std::endl;
                break;
            }
        }

        bridge.header.stamp = ros::Time::now();
        bridge.encoding = "bgr8";
        bridge.image = frame;
        image_pub.publish(bridge.toImageMsg());
        
        fps_counter.displayFPS(frame);
        cv::imshow("video", frame);

        char key = cv::waitKey(1);
        if (key == 'q') {
            break;
        } else if (key == 's') {
            std::time_t t = std::time(nullptr);
            char time_str[100];
            std::strftime(time_str, sizeof(time_str), "%Y-%m-%d_%H_%M_%S", std::localtime(&t));
            output_path = std::to_string(width) + "x" + std::to_string(height) + "_" + time_str + ".jpg";
            cv::imwrite(output_path, frame);
            std::cout << "Save success, file name: " << output_path << std::endl;
        }
    }

    camera.release();
    cv::destroyAllWindows();
    return 0;
}