///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2025, 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.
//
///////////////////////////////////////////////////////////////////////////

// ZED include
#include "ClientPublisher.hpp"
#include "GLViewer.hpp"

#include <memory>

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

#ifdef _SL_JETSON_
  const bool isJetson = true;
#else
  const bool isJetson = false;
#endif

  if (argc != 2) {
    // this file should be generated by using the tool ZED360
    std::cout << "Need a Configuration file in input" << std::endl;
    return 1;
  }

  // Defines the Coordinate system and unit used in this sample
  constexpr sl::COORDINATE_SYSTEM COORDINATE_SYSTEM =
      sl::COORDINATE_SYSTEM::RIGHT_HANDED_Y_UP;
  constexpr sl::UNIT UNIT = sl::UNIT::METER;

  // Read json file containing the configuration of your multicamera setup.
  auto configurations =
      sl::readFusionConfigurationFile(argv[1], COORDINATE_SYSTEM, UNIT);

  if (configurations.empty()) {
    std::cout << "Empty configuration File." << std::endl;
    return EXIT_FAILURE;
  }

  Trigger trigger;

  // Open camera streams from hard-coded configuration
  std::vector<std::unique_ptr<ClientPublisher>> clients;
  int id_ = 0;
  int gpu_id = 0;

  int nb_gpu = 0;
  if (!isJetson)
    cudaGetDeviceCount(&nb_gpu);

  // Use hard-coded camera stream configurations
  for (const auto& cam_config : CAMERA_STREAM_CONFIGS) {
    std::cout << "Try to open stream for ZED " << cam_config.serial_number
              << " at " << cam_config.ip_address << ":" << cam_config.port << ".."
              << std::flush;

    gpu_id = (nb_gpu == 0) ? 0 : id_ % nb_gpu;

    auto client = std::make_unique<ClientPublisher>();
    auto state = client->open(cam_config.ip_address, cam_config.port, &trigger, gpu_id);
    if (!state) {
      std::cerr << "Could not open stream for ZED: " << cam_config.serial_number
                << ". Skipping..." << std::endl;
      continue;
    }

    std::cout << ". ready ! (Serial: " << client->getSerialNumber() << ")" << std::endl;

    clients.push_back(std::move(client));
    id_++;
  }

  // start camera threads
  for (auto &it : clients) {
    it->start();
  }

  // Now that the ZED camera are running, we need to initialize the fusion
  // module
  sl::InitFusionParameters init_params;
  init_params.coordinate_units = UNIT;
  init_params.coordinate_system = COORDINATE_SYSTEM;

  sl::Resolution low_res(512, 360);
  init_params.maximum_working_resolution = low_res;
  // create and initialize it
  sl::Fusion fusion;
  fusion.init(init_params);

  // subscribe to every cameras of the setup to internally gather their data
  std::vector<sl::CameraIdentifier> cameras;
  for (auto &it : configurations) {
    sl::CameraIdentifier uuid(it.serial_number);
    // to subscribe to a camera you must give its serial number, the way to
    // communicate with it (shared memory or local network), and its world pose
    // in the setup.
    auto state = fusion.subscribe(uuid, it.communication_parameters, it.pose,
                                  it.override_gravity);
    if (state != sl::FUSION_ERROR_CODE::SUCCESS)
      std::cout << "Unable to subscribe to " << std::to_string(uuid.sn) << " . "
                << state << std::endl;
    else
      cameras.push_back(uuid);
  }

  // check that at least one camera is connected
  if (cameras.empty()) {
    std::cout << "no connections " << std::endl;
    return EXIT_FAILURE;
  }

  // as this sample shows how to fuse body detection from the multi camera setup
  // we enable the Body Tracking module with its options
  sl::BodyTrackingFusionParameters body_fusion_init_params;
  body_fusion_init_params.enable_tracking = true;
  body_fusion_init_params.enable_body_fitting =
      !isJetson; // skeletons will looks more natural but requires more
                 // computations
  fusion.enableBodyTracking(body_fusion_init_params);

  // define fusion behavior
  sl::BodyTrackingFusionRuntimeParameters body_tracking_runtime_parameters;
  // be sure that the detection skeleton is complete enough
  body_tracking_runtime_parameters.skeleton_minimum_allowed_keypoints = 7;
  // we can also want to retrieve skeleton seen by multiple camera, in this case
  // at least half of them
  body_tracking_runtime_parameters.skeleton_minimum_allowed_camera =
      cameras.size() / 2.;
  body_tracking_runtime_parameters.skeleton_smoothing = 0.1f;

  // creation of a 3D viewer
  GLViewer viewer;
  viewer.init(argc, argv);

  std::cout << "Viewer Shortcuts\n"
            << "\t- 'q': quit the application\n"
            << "\t- 'r': switch on/off for raw skeleton display\n"
            << "\t- 'p': switch on/off for live point cloud display\n"
            << "\t- 'c': switch on/off point cloud display with raw color\n"
            << std::endl;

  // fusion outputs
  sl::Bodies fused_bodies;
  std::map<sl::CameraIdentifier, sl::Bodies> camera_raw_data;
  sl::FusionMetrics metrics;
  std::map<sl::CameraIdentifier, sl::Mat> views;
  std::map<sl::CameraIdentifier, sl::Mat> pointClouds;
  sl::CameraIdentifier fused_camera(0);

  // run the fusion as long as the viewer is available.
  while (viewer.isAvailable()) {
    trigger.notifyZED();

    // run the fusion process (which gather data from all camera, sync them and
    // process them)
    if (fusion.process() == sl::FUSION_ERROR_CODE::SUCCESS) {

      // Retrieve fused body
      fusion.retrieveBodies(fused_bodies, body_tracking_runtime_parameters);
      // for debug, you can retrieve the data sent by each camera
      for (auto &id : cameras) {
        fusion.retrieveBodies(camera_raw_data[id],
                              body_tracking_runtime_parameters, id);

        auto state_view = fusion.retrieveImage(views[id], id, low_res);
        auto state_pc = fusion.retrieveMeasure(pointClouds[id], id,
                                               sl::MEASURE::XYZBGRA, low_res);

        if (state_view == sl::FUSION_ERROR_CODE::SUCCESS &&
            state_pc == sl::FUSION_ERROR_CODE::SUCCESS)
          viewer.updateCamera(id.sn, views[id], pointClouds[id]);

        sl::Pose pose;
        if (fusion.getPosition(pose, sl::REFERENCE_FRAME::WORLD, id,
                               sl::POSITION_TYPE::RAW) ==
            sl::POSITIONAL_TRACKING_STATE::OK)
          viewer.setCameraPose(id.sn, pose.pose_data);
      }

      // get metrics about the fusion process for monitoring purposes
      fusion.getProcessMetrics(metrics);
    }
    // update the 3D view
    viewer.updateBodies(fused_bodies, camera_raw_data, metrics);
  }

  trigger.running = false;
  trigger.notifyZED();

  for (auto &it : clients)
    it->stop();

  fusion.close();

  return EXIT_SUCCESS;
}