#include <cstdio>
#include <fstream>
#include <iostream>
#include <string>
#include <thread>
#include <vector>
#include <chrono>

#include <unitree/robot/go2/video/video_client.hpp>
#include <unitree/robot/channel/channel_factory.hpp>

int main(int argc, char** argv) {
    if (argc < 2) {
        std::cerr << "Usage: vision_node <network_interface>\n";
        return 1;
    }

    const std::string frame_path = "/dev/shm/robot_frame.jpg";
    const std::string temp_path = "/dev/shm/robot_frame.tmp.jpg";

    unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
    unitree::robot::go2::VideoClient video;
    video.SetTimeout(1.0f);
    video.Init();

    std::cout << "[SYS] Writing camera frames to " << frame_path << std::endl;

    std::vector<uint8_t> image;
    while (true) {
        const int ret = video.GetImageSample(image);
        if (ret != 0 || image.empty()) {
            std::cerr << "[WARN] GetImageSample failed: " << ret << std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(33));
            continue;
        }

        bool ok = false;
        {
            std::ofstream f(temp_path, std::ios::binary | std::ios::trunc);
            if (f) {
                f.write(reinterpret_cast<const char*>(image.data()),
                        static_cast<std::streamsize>(image.size()));
                f.flush();
                ok = f.good();
            }
        }

        if (ok && std::rename(temp_path.c_str(), frame_path.c_str()) != 0) {
            std::perror("[ERROR] rename");
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(33));
    }
}
