/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#pragma once

#include <mrpt/img/CImage.h>
#include <mrpt/img/TStereoCamera.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/vision/chessboard_camera_calib.h>
#include <mrpt/vision/types.h>

#include <array>

namespace mrpt::vision
{
/** \addtogroup chessboard_calib Chessboard calibration
 *  \ingroup mrpt_vision_grp
 *  @{  */

/** Data associated to each stereo image in the calibration process
 * mrpt::vision::checkerBoardCameraCalibration (All the information can be left
 * empty and will be filled up in the calibration method).
 */
struct TImageStereoCalibData
{
  TImageCalibData left, right;

  /** Empty all the data */
  void clear() { *this = TImageStereoCalibData(); }
};

/** Params of the optional callback provided by the user */
struct TImageStereoCallbackData
{
  /** =-1:Processing images;  =0: Initial calib without distortion, =1: Calib
   * of all parameters */
  int calibRound{-1};
  size_t current_iter{0};
  /** Current root-mean square reprojection error (in pixels) */
  double current_rmse{0};
  /** Info for calibRound==-1 */
  unsigned int nImgsProcessed{0}, nImgsToProcess{0};
};

/** Prototype of optional user callback function. */
using TSteroCalibCallbackFunctor = void (*)(const TImageStereoCallbackData& d, void* user_data);

/** Input parameters for mrpt::vision::checkerBoardStereoCalibration */
struct TStereoCalibParams
{
  /** The number of squares in the checkerboard in the "X" & "Y" direction. */
  unsigned int check_size_x{7}, check_size_y{9};
  /** The size of each square in the checkerboard, in meters, in the "X" & Y"
   * axes. */
  double check_squares_length_X_meters{0.02}, check_squares_length_Y_meters{0.02};
  bool normalize_image{true};
  bool skipDrawDetectedImgs{false};
  /** Show progress messages to std::cout console (default=true) */
  bool verbose{true};
  /** Maximum number of iterations of the optimizer (default=300) */
  size_t maxIters{2000};

  /** Select which distortion parameters (of both left/right cameras) will be
   * optimzed:
   *  k1,k2,k3 are the r^2, r^4 and r^6 radial distorion coeficients, and t1
   * and t2 are the tangential distortion coeficients (see
   * mrpt::img::TCamera).
   * Those set to false will be assumed to be fixed to zero (no distortion).
   * \note Default values are to only assume distortion via k1 and k2 (the
   * rest are zeros).
   */
  bool optimize_k1{true}, optimize_k2{true}, optimize_k3{false}, optimize_t1{false},
      optimize_t2{false};

  /** Employ a Pseudo-Huber robustifier kernel (Default: true) */
  bool use_robust_kernel = true;
  /** The parameter of the robust kernel, in pixels (only if
   * use_robust_kernel=true) (Default=5) */
  double robust_kernel_param = 5;

  /** If set to !=NULL, this function will be called within each Lev-Marq.
   * iteration (don't do heavy stuff here since performance will degrade) */
  TSteroCalibCallbackFunctor callback{nullptr};
  /** If using a callback function, you can use this to pass custom data to
   * your callback. */
  void* callback_user_param{nullptr};

  // Ctor: Set default values
  TStereoCalibParams();
};

/** Output results for mrpt::vision::checkerBoardStereoCalibration */
struct TStereoCalibResults
{
  TStereoCalibResults();

  /** Recovered parameters of the stereo camera */
  mrpt::img::TStereoCamera cam_params;
  /** The pose of the left camera as seen from the right camera */
  mrpt::poses::CPose3D right2left_camera_pose;

  /** Poses of the origin of coordinates of the pattern wrt the left camera
   * (i.e. the origin of coordinates, as seen from the different camera poses)
   */
  std::vector<mrpt::math::TPose3D> left_cam_poses;
  /** true if a checkerboard was correctly detected in both left/right images.
   * false if it wasn't, so the image pair didn't make it to the optimization.
   */
  std::vector<bool> image_pair_was_used;

  /** Final reprojection square Root Mean Square Error (in pixels). */
  double final_rmse{0};
  /** Final number of optimization iterations executed. */
  size_t final_iters{0};
  /** Number of image pairs in which valid checkerboards were correctly
   * detected. */
  size_t final_number_good_image_pairs{0};

  /** The inverse variance (information/precision) of each of the 9 left/right
   * camera parameters [fx fy cx cy k1 k2 k3 t1 t2].
   *  Those not estimated as indicated in TStereoCalibParams will be zeros
   * (i.e. an "infinite uncertainty")
   */
  std::array<double, 9> left_params_inv_variance, right_params_inv_variance;
};

/**  A list of images, used in checkerBoardStereoCalibration
 * \sa checkerBoardStereoCalibration
 */
using TCalibrationStereoImageList = std::vector<TImageStereoCalibData>;

/** Optimize the calibration parameters of a stereo camera or a RGB+D (Kinect)
 * camera.
 *  This computes the projection and distortion parameters of each camera, and
 * their relative spatial pose,
 *  from a sequence of pairs of captured images of a checkerboard.
 *  A custom implementation of an optimizer (Levenberg-Marquartd) seeks for the
 * set of selected parameters to estimate that minimize the reprojection errors.
 *
 *  \param input_images [IN/OUT] At input, this list must have one entry for
 * each image to process. At output the original, detected checkboard and
 * rectified images can be found here. See TImageCalibData.
 *  \param params [IN] Mandatory: the user must provide the size of the
 * checkerboard, which parameters to optimize and which to leave fixed to zero,
 * etc.
 *  \param out_results [OUT] The results of the calibration, and its
 * uncertainty measure, will be found here upon return.
 *
 * \return false on any error (more info will be dumped to cout), or true on
 * success.
 * \note See also the ready-to-use application: <a
 * href="http://www.mrpt.org/list-of-mrpt-apps/application-kinect-calibrate"
 * >kinect-calibrate</a>
 * \sa CImage::findChessboardCorners, checkerBoardCameraCalibration,
 * mrpt::hwdrivers::CKinect
 */
bool checkerBoardStereoCalibration(
    TCalibrationStereoImageList& images,
    const TStereoCalibParams& params,
    TStereoCalibResults& out_results);

/** @}  */  // end of grouping
}  // namespace mrpt::vision
