#include <stdio.h>
#include <string.h>

#include <sl/Camera.hpp>
#include <sl/CameraOne.hpp>

#include <opencv2/opencv.hpp>

using namespace std;
using namespace sl;

void printHelp();
void printDevices();

VIDEO_SETTINGS camera_settings_ = VIDEO_SETTINGS::BRIGHTNESS;

int main(int argc, char **argv) {
    Camera zed;

    sl::InitParameters init_parameters;
    init_parameters.sdk_verbose = true;
    init_parameters.camera_resolution = sl::RESOLUTION::AUTO;
    init_parameters.depth_mode = sl::DEPTH_MODE::NONE; // no depth computation required here
    printDevices();

    unsigned int serial_left = 316310960;  // Left camera serial number
    unsigned int serial_right = 310559220; // Right camera serial number
    unsigned int virt_sn = generateVirtualStereoSerialNumber(serial_left, serial_right);
    init_parameters.input.setVirtualStereoFromSerialNumbers(serial_left, serial_right, virt_sn);
    
    auto returned_state = zed.open(init_parameters);
    if (returned_state == ERROR_CODE::INVALID_CALIBRATION_FILE) {
        std::cout << "WARNING: Virtual Stereo rig not calibrated, only unrectified images and recording are available" << std::endl;
    }else if (returned_state != ERROR_CODE::SUCCESS) {
        std::cout << ("Camera Open", returned_state, "Exit program.") << std::endl;
        return EXIT_FAILURE;
    }

    StreamingParameters stream_params;
    stream_params.port = 34000;

    returned_state = zed.enableStreaming(stream_params);
    if (returned_state != ERROR_CODE::SUCCESS) {
        std::cout << ("Streaming initialization error: ", returned_state) << std::endl;
        return EXIT_FAILURE;
    }


    auto camera_info = zed.getCameraInformation();
    auto camera_conf = camera_info.camera_configuration;
    cout << endl;
    cout << "ZED Model                 : " << camera_info.camera_model << endl;
    cout << "ZED Serial Number         : " << camera_info.serial_number << endl;
    cout << "ZED Camera Firmware       : " << camera_conf.firmware_version << "/" << camera_info.sensors_configuration.firmware_version << endl;
    cout << "ZED Camera Resolution     : " << camera_conf.resolution.width << "x" << camera_conf.resolution.height << endl;
    cout << "ZED Camera FPS            : " << zed.getInitParameters().camera_fps << endl;


    int fcount = 0;

    while (true) {
        if (zed.grab() <= ERROR_CODE::SUCCESS)
            fcount++;

        sleep_ms(1);
    } 
    // Exit
    zed.disableStreaming();
    zed.close();
    return EXIT_SUCCESS;
}


void printDevices() {       
    auto devices = sl::CameraOne::getDeviceList();
        
    if (devices.empty()) {
        std::cout << "No ZED cameras found." << std::endl;
        return;
    }
    for(auto &it:devices){
        if(it.camera_state == sl::CAMERA_STATE::AVAILABLE && isCameraOne(it.camera_model)) { 
            std::cout << "- ZED One camera: " << it.camera_model << " (ID : " << it.id << ", Serial: " << it.serial_number << ")" << std::endl;
        }
    }
}