///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2024, STEREOLABS.
//
// All rights reserved.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////

/************************************************************************************************
 ** This sample shows how to stream remotely the video of a ZED camera. Any application using   **
 ** the ZED SDK can receive and process this stream. See Camera Streaming/Receiver example.     **
 *************************************************************************************************/

// ZED includes
#include <sl/Camera.hpp>


// Sample includes
// #include "utils.hpp"
#include <atomic>
#include <csignal>

std::atomic<bool> exit_app(false);

// Using namespace
using namespace sl;
using namespace std;

void print(string msg_prefix, ERROR_CODE err_code = ERROR_CODE::SUCCESS, string msg_suffix = "");
int parseArgs(int argc, char **argv, sl::InitParameters& param);
// void zed_stream(Camera& zed, bool& run);
// void zed_stream(Camera& zed, bool& exit_app);
void zed_stream(Camera& zed, std::atomic<bool>& exit_app);

void signalHandler(int signum) {
    std::cout << "\nSignal (" << signum << ") received. Exiting...\n";
    exit_app.store(true); // Set the atomic flag to true
}


int main(int argc, char **argv) {
    // SetCtrlHandler();
    std::signal(SIGINT, signalHandler);

    // Set configuration parameters for the ZED
    InitParameters init_parameters;
    init_parameters.camera_resolution = sl::RESOLUTION::AUTO;
    init_parameters.depth_mode = DEPTH_MODE::NONE;
    init_parameters.sdk_verbose = 2;
    int res_arg = parseArgs(argc, argv, init_parameters);

    vector< DeviceProperties> devList = Camera::getDeviceList();
    int nb_detected_zed = devList.size();

	for (int z = 0; z < nb_detected_zed; z++) {
		std::cout << "ID : " << devList[z].id << " ,model : " << devList[z].camera_model << " , S/N : " << devList[z].serial_number << " , state : "<<devList[z].camera_state<<std::endl;
	}
	
    if (nb_detected_zed == 0) {
        cout << "No ZED Detected, exit program" << endl;
        return EXIT_FAILURE;
    }
    
    cout << nb_detected_zed << " ZED Detected" << endl;

    // Set the streaming parameters
    vector<sl::StreamingParameters> stream_params(nb_detected_zed);
    stream_params[0].codec = sl::STREAMING_CODEC::H265; // Can be H264 or H265
    stream_params[0].bitrate = 8000;
    stream_params[0].port = 30000; // Port used for sending the stream

    stream_params[1].codec = sl::STREAMING_CODEC::H265; // Can be H264 or H265
    stream_params[1].bitrate = 8000;
    stream_params[1].port = 30002; // Port used for sending the stream

    stream_params[2].codec = sl::STREAMING_CODEC::H265; // Can be H264 or H265
    stream_params[2].bitrate = 8000;
    stream_params[2].port = 30004; // Port used for sending the stream

    stream_params[3].codec = sl::STREAMING_CODEC::H265; // Can be H264 or H265
    stream_params[3].bitrate = 8000;
    stream_params[3].port = 40000; // Port used for sending the stream

    vector<Camera> zeds(nb_detected_zed);
    // try to open every detected cameras
    for (int z = 0; z < nb_detected_zed; z++) {
        init_parameters.input.setFromSerialNumber(devList[z].serial_number);
        init_parameters.camera_fps = 30;
        ERROR_CODE err = zeds[z].open(init_parameters);
        if (err == ERROR_CODE::SUCCESS) {
            auto cam_info = zeds[z].getCameraInformation();
            cout << cam_info.camera_model << ", ID: " << z << ", SN: " << cam_info.serial_number << " Opened" << endl;
        } else {
            cout << "ZED ID:" << z << " Error: " << err << endl;
            zeds[z].close();
        }
    }

    // // Enable streaming on all cameras
    for (int z = 0; z < nb_detected_zed; z++) {
        auto returned_state = zeds[z].enableStreaming(stream_params[z]);
        if (returned_state != ERROR_CODE::SUCCESS) {
            print("Streaming initialization error: ", returned_state);
            return EXIT_FAILURE;
        }
        print("Camera " + to_string(devList[z].serial_number) + " streaming on port " + to_string(stream_params[z].port));
    }

    // Create a grab thread for each opened camera
    vector<thread> thread_pool(nb_detected_zed); // compute threads

    bool zed_open = false;
    for (int z = 0; z < nb_detected_zed; z++)
        if (zeds[z].isOpened()) {
            // give each streaming camera its own thread
            // thread_pool[z] = std::thread(zed_stream, ref(zeds[z]), ref(exit_app));
            thread_pool[z] = std::thread(zed_stream, std::ref(zeds[z]), std::ref(exit_app));
            zed_open = true;
        }

    if(!zed_open) {        
        cout << "No ZED opened, exit program" << endl;
        return EXIT_FAILURE;
    }

    while (!exit_app.load()) {
        sleep_ms(100);
    }

    
    // Wait for every thread to be stopped
    for (int z = 0; z < nb_detected_zed; z++)
        if (zeds[z].isOpened()) {
            thread_pool[z].join();
            cout << "ZED: " << to_string(devList[z].serial_number) << " re-joining main thread." << endl;
    }

    for (int z=0; z < nb_detected_zed; z++)
        if (zeds[z].isOpened()) {
            zeds[z].disableStreaming();
            cout << "ZED: " << to_string(devList[z].serial_number) << " streaming disabled" << endl;     
    }

    for (int z = 0; z < nb_detected_zed; z++)
        if (zeds[z].isOpened()) {
            zeds[z].close();
            cout << "ZED: " << to_string(devList[z].serial_number) << " connection closed." << endl;
    }

    cout << "Good bye" << endl;
    return EXIT_SUCCESS;
}

// void zed_stream(Camera& zed, bool& exit_app) {
//     while (!exit_app) {
//         if (zed.grab() != ERROR_CODE::SUCCESS)
//             sleep_ms(1);
//     }
// }
void zed_stream(Camera& zed, std::atomic<bool>& exit_app) {
    while (!exit_app.load()) {
        if (zed.grab() != ERROR_CODE::SUCCESS) {
            sleep_ms(1);
        }
    }
}

void print(string msg_prefix, ERROR_CODE err_code, string msg_suffix) {
    cout << "[Sample]";
    if (err_code != ERROR_CODE::SUCCESS)
        cout << "[Error] ";
    else
        cout << " ";
    cout << msg_prefix << " ";
    if (err_code != ERROR_CODE::SUCCESS) {
        cout << " | " << toString(err_code) << " : ";
        cout << toVerbose(err_code);
    }
    if (!msg_suffix.empty())
        cout << " " << msg_suffix;
    cout << endl;
}

int parseArgs(int argc, char **argv, sl::InitParameters& param) {
    if (argc > 1 && string(argv[1]).find(".svo") != string::npos) {
        // SVO input mode
        param.input.setFromSVOFile(argv[1]);
        cout << "[Sample] Using SVO File input: " << argv[1] << endl;
    } else if (argc > 1 && string(argv[1]).find(".svo") == string::npos) {
        string arg = string(argv[1]);
        unsigned int a, b, c, d, port;
        if (sscanf(arg.c_str(), "%u.%u.%u.%u:%d", &a, &b, &c, &d, &port) == 5) {
            // Stream input mode - IP + port
            string ip_adress = to_string(a) + "." + to_string(b) + "." + to_string(c) + "." + to_string(d);
            param.input.setFromStream(sl::String(ip_adress.c_str()), port);
            cout << "[Sample] Using Stream input, IP : " << ip_adress << ", port : " << port << endl;
        } else if (sscanf(arg.c_str(), "%u.%u.%u.%u", &a, &b, &c, &d) == 4) {
            // Stream input mode - IP only
            param.input.setFromStream(sl::String(argv[1]));
            cout << "[Sample] Using Stream input, IP : " << argv[1] << endl;
        } else if (arg.find("HD2K") != string::npos) {
            param.camera_resolution = RESOLUTION::HD2K;
            cout << "[Sample] Using Camera in resolution HD2K" << endl;
        }else if (arg.find("HD1200") != string::npos) {
            param.camera_resolution = RESOLUTION::HD1200;
            cout << "[Sample] Using Camera in resolution HD1200" << endl;
        } else if (arg.find("HD1080") != string::npos) {
            param.camera_resolution = RESOLUTION::HD1080;
            cout << "[Sample] Using Camera in resolution HD1080" << endl;
        } else if (arg.find("HD720") != string::npos) {
            param.camera_resolution = RESOLUTION::HD720;
            cout << "[Sample] Using Camera in resolution HD720" << endl;
        }else if (arg.find("SVGA") != string::npos) {
            param.camera_resolution = RESOLUTION::SVGA;
            cout << "[Sample] Using Camera in resolution SVGA" << endl;
        }else if (arg.find("VGA") != string::npos) {
            param.camera_resolution = RESOLUTION::VGA;
            cout << "[Sample] Using Camera in resolution VGA" << endl;
        } else
            return 1;
    } else {
        // Default
        return 1;
    }
    return 0;
}