#include <sl/Camera.hpp>

#include <iostream>
#include <string>
#include <signal.h>

bool still_going = true;

void handler(int s)
{
  std::cout << "Caught signal ... terminating" << std::endl;
  still_going = false;
}

int main(int argc, char **argv) 
{
  signal(SIGINT, handler);
  
  // Create a ZED camera object
  sl::Camera zed;

  // Set configuration parameters
  sl::InitParameters init_parameters;
  init_parameters.camera_resolution   = sl::RESOLUTION::HD1080; // Use HD1080 video mode
  init_parameters.camera_fps    = 60; // 60 fps
  init_parameters.camera_image_flip   = sl::FLIP_MODE::OFF; //Camera right-side up
  init_parameters.coordinate_system   = sl::COORDINATE_SYSTEM::IMAGE; //Image RDF
  init_parameters.depth_mode    = sl::DEPTH_MODE::NONE; // No depth for data collection


  // Open the camera
  auto ret = zed.open(init_parameters);
  if (ret != sl::ERROR_CODE::SUCCESS) {
      std::cout << "Error " << ret << ", exit program." << std::endl;
      return EXIT_FAILURE;
  }

  // Auto exposure/gain
  sl::Rect roi;
  roi.x = 835;
  roi.y = 415;
  roi.width = 500;
  roi.height = 500;
  sl::SIDE side = sl::SIDE::LEFT; 
  
  if ((ret = zed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, 1)) != sl::ERROR_CODE::SUCCESS)
    std::cout << "Set ROI on failed: " << sl::toVerbose(ret).c_str() << std::endl;

  //keep grabbing frames until stop is triggered
  auto start = std::chrono::system_clock::now();
  while(still_going)
  {
    // Step down ROI width and height until limit
	  roi.width = std::clamp(int(roi.width - 1), 256, 500);
    roi.height = std::clamp(int(roi.height - 1), 256, 500);
    std::cout << "Set ROI: " << roi.x << " " << roi.y << " " << roi.width << " " << roi.height << std::endl;


    if ((ret = zed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC_ROI, roi, side)) != sl::ERROR_CODE::SUCCESS)
	    std::cout << "Set ROI failed: " << sl::toVerbose(ret).c_str() << std::endl;

    std::cout << "Before ZED Grab" << std::endl;
    ret = zed.grab();
    std::cout << "After ZED Grab" << std::endl;
    if (ret == sl::ERROR_CODE::SUCCESS) 
    {
      std::cout << "Grabbed latest frame"  << std::endl;
    }
    else 
    {
      std::cout << "Grab failed " << sl::toVerbose(ret).c_str() << std::endl;
    }
  }
  // Close the camera
  zed.close();
  std::cout << "Closed camera" << std::endl;
  return EXIT_SUCCESS;
}