///////////////////////////////////////////////////////////////////////////
//
// 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.
//
///////////////////////////////////////////////////////////////////////////

/************************************************************************************************
 ** Stereo pair streaming sender using a virtual stereo created from two ZED X One cameras.    **
 ** Serial numbers are hardcoded to match the setup in                                          **
 ** video-recording/recording/mono/cpp/src/main.cpp                                             **
 *************************************************************************************************/

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

// STL includes
#include <algorithm>
#include <cctype>

// Sample includes
#include "utils.hpp"

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

void print(const string &msg_prefix, ERROR_CODE err_code = ERROR_CODE::SUCCESS, const string &msg_suffix = "");
void parseArgs(int argc, char **argv, InitParameters &init_parameters, StreamingParameters &stream_params);

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

    InitParameters init_parameters;
    init_parameters.camera_resolution = RESOLUTION::HD1080;
    init_parameters.depth_mode = DEPTH_MODE::NONE;
    init_parameters.sdk_verbose = 1;
    init_parameters.svo_real_time_mode = 1;
    init_parameters.camera_fps = 60.0;

    // Hardcoded virtual stereo pair configuration.
    constexpr unsigned int left_camera_sn = 302631713;
    constexpr unsigned int right_camera_sn = 305821220;
    constexpr int stereo_pair_sn = 113866047;
    init_parameters.input.setVirtualStereoFromSerialNumbers(left_camera_sn, right_camera_sn, stereo_pair_sn);

    StreamingParameters stream_params;
    stream_params.port = 30000;
    stream_params.codec = STREAMING_CODEC::H265;
    stream_params.bitrate = 22000;
    parseArgs(argc, argv, init_parameters, stream_params);

    auto returned_state = zed.open(init_parameters);
    if (returned_state != ERROR_CODE::SUCCESS) {
        print("Camera Open", returned_state, "Exit program.");
        return EXIT_FAILURE;
    }

    returned_state = zed.enableStreaming(stream_params);
    if (returned_state != ERROR_CODE::SUCCESS) {
        print("Streaming initialization error:", returned_state);
        zed.close();
        return EXIT_FAILURE;
    }

    print("Virtual stereo streaming on port " + to_string(stream_params.port));

    SetCtrlHandler();
    while (!exit_app) {
        if (zed.grab() != ERROR_CODE::SUCCESS) {
            sleep_ms(1);
        }
    }

    zed.disableStreaming();
    zed.close();
    return EXIT_SUCCESS;
}

void print(const string &msg_prefix, ERROR_CODE err_code, const 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) << " : " << toVerbose(err_code);
    }
    if (!msg_suffix.empty()) {
        cout << " " << msg_suffix;
    }
    cout << endl;
}

void parseArgs(int argc, char **argv, InitParameters &init_parameters, StreamingParameters &stream_params) {
    for (int i = 1; i < argc; ++i) {
        string arg(argv[i]);

        if (arg == "HD1200") {
            init_parameters.camera_resolution = RESOLUTION::HD1200;
            cout << "[Sample] Using Camera resolution HD1200" << endl;
        } else if (arg == "HD1080") {
            init_parameters.camera_resolution = RESOLUTION::HD1080;
            cout << "[Sample] Using Camera resolution HD1080" << endl;
        } else if (arg == "SVGA") {
            init_parameters.camera_resolution = RESOLUTION::SVGA;
            cout << "[Sample] Using Camera resolution SVGA" << endl;
        } else {
            // If argument is numeric, treat it as port.
            bool numeric = !arg.empty() && all_of(arg.begin(), arg.end(), ::isdigit);
            if (numeric) {
                stream_params.port = static_cast<uint16_t>(stoi(arg));
                cout << "[Sample] Using streaming port " << stream_params.port << endl;
            }
        }
    }
}