// Copyright (c), ETH Zurich and UNC Chapel Hill.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
//       its contributors may be used to endorse or promote products derived
//       from this software without specific prior written permission.
//
// 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 HOLDERS 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.

#pragma once

#include "colmap/geometry/rigid3.h"
#include "colmap/scene/reconstruction.h"
#include "colmap/sensor/bitmap.h"

namespace colmap {

struct UndistortCameraOptions {
  // The amount of blank pixels in the undistorted image in the range [0, 1].
  double blank_pixels = 0.0;

  // Minimum and maximum scale change of camera used to satisfy the blank
  // pixel constraint.
  double min_scale = 0.2;
  double max_scale = 2.0;

  // Maximum image size in terms of width or height of the undistorted camera.
  int max_image_size = -1;

  // The 4 factors in the range [0, 1] that define the ROI (region of interest)
  // in original image. The bounding box pixel coordinates are calculated as
  //    (roi_min_x * Width, roi_min_y * Height) and
  //    (roi_max_x * Width, roi_max_y * Height).
  double roi_min_x = 0.0;
  double roi_min_y = 0.0;
  double roi_max_x = 1.0;
  double roi_max_y = 1.0;

  // Maximum norm of the undistorted camera-space point (= tan(theta) where
  // theta is the angle from the optical axis) used when tracing border pixels
  // to compute the output image dimensions. For fisheye cameras with off-center
  // principal points, border pixels outside the valid fisheye circle can have
  // theta approaching pi/2, causing tan(theta) to diverge and producing extreme
  // output dimensions. Points with norm exceeding this threshold are skipped.
  // Set to -1 to disable the check (default).
  double max_cam_point_norm = -1;
};

// Undistort camera by resizing the image and shifting the principal point.
//
// The scaling factor is computed such that no blank pixels are in the
// undistorted image (blank_pixels=0) or all pixels in distorted image are
// contained in output image (blank_pixels=1).
//
// The focal length of the image is preserved and the dimensions of the
// undistorted pinhole camera are adjusted such that either all pixels in
// the undistorted image have a corresponding pixel in the distorted image
// (i.e. no blank pixels at the borders, for `blank_pixels=0`), or all pixels
// in the distorted image project have a corresponding pixel in the undistorted
// image (i.e. blank pixels at the borders, for `blank_pixels=1`). Intermediate
// states can be achieved by setting `blank_pixels` between 0 and 1.
//
// The relative location of the principal point of the distorted camera is
// preserved. The scaling of the image dimensions is subject to the `min_scale`,
// `max_scale`, and `max_image_size` constraints.
Camera UndistortCamera(const UndistortCameraOptions& options,
                       const Camera& camera);

// Undistort image such that the viewing geometry of the undistorted image
// follows a pinhole camera model. See `UndistortCamera` for more details
// on the undistortion conventions.
void UndistortImage(const UndistortCameraOptions& options,
                    const Bitmap& distorted_image,
                    const Camera& distorted_camera,
                    Bitmap* undistorted_image,
                    Camera* undistorted_camera);

// Undistort all cameras in the reconstruction and accordingly all
// observations in their corresponding images.
void UndistortReconstruction(const UndistortCameraOptions& options,
                             Reconstruction* reconstruction);

// Compute stereo rectification homographies that transform two images,
// such that corresponding pixels in one image lie on the same scanline in the
// other image. The matrix Q transforms disparity values to world coordinates
// as [x, y, disparity, 1] * Q = [X, Y, Z, 1] * w. Note that this function
// assumes that the two cameras are already undistorted.
void RectifyStereoCameras(const Camera& camera1,
                          const Camera& camera2,
                          const Rigid3d& cam2_from_cam1,
                          Eigen::Matrix3d* H1,
                          Eigen::Matrix3d* H2,
                          Eigen::Matrix4d* Q);

// Rectify and undistort the stereo image pair using the given geometry.
void RectifyAndUndistortStereoImages(const UndistortCameraOptions& options,
                                     const Bitmap& distorted_image1,
                                     const Bitmap& distorted_image2,
                                     const Camera& distorted_camera1,
                                     const Camera& distorted_camera2,
                                     const Rigid3d& cam2_from_cam1,
                                     Bitmap* undistorted_image1,
                                     Bitmap* undistorted_image2,
                                     Camera* undistorted_camera,
                                     Eigen::Matrix4d* Q);

}  // namespace colmap
