/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
// M*//////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this
// license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without
// modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's 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.
//
// * The name of the copyright holders may not 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 Intel Corporation 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.
//
// M*/

/****************************************************************************************\
* Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation.
* Contributed by Edgar Riba
\****************************************************************************************/

#pragma once

#include <mrpt/3rdparty/do_opencv_includes.h>
#include <mrpt/config.h>

#if MRPT_HAS_OPENCV
//#include <opencv2/core/core_c.h>
#include <iostream>

namespace mrpt::vision::pnp
{
/** \addtogroup pnp Perspective-n-Point pose estimation
 *  \ingroup mrpt_vision_grp
 *  @{
 */

/**
 * @class upnp
 * @author Chandra Mangipudi
 * @date 12/08/16
 * @file upnp.h
 * @brief Unified PnP - Eigen Wrapper for OpenCV function
 */
class upnp
{
   public:
	//! Constructor for UPnP class
	upnp(
		const cv::Mat& cameraMatrix, const cv::Mat& opoints,
		const cv::Mat& ipoints);

	//! Destructor for UPnP class
	~upnp();

	/**
	 * @brief Function to compute pose
	 * @param[out] R Rotation Matrix
	 * @param[out] t Translation Vector
	 * @return
	 */
	double compute_pose(cv::Mat& R, cv::Mat& t);

   private:
	/**
	 * @brief Initialize camera variables using camera intrinsic matrix
	 * @param[in] cameraMatrix Camera Intrinsic Matrix
	 */
	template <typename T>
	void init_camera_parameters(const cv::Mat& cameraMatrix)
	{
		uc = cameraMatrix.at<T>(0, 2);
		vc = cameraMatrix.at<T>(1, 2);
		fu = 1;
		fv = 1;
	}

	/**
	 * @brief Iniialize Object points and image points from OpenCV Matrix
	 * @param[in] opoints Object Points
	 * @param[in] ipoints Image Points
	 */
	template <typename OpointType, typename IpointType>
	void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
	{
		for (int i = 0; i < number_of_correspondences; i++)
		{
			pws[3 * i] = opoints.at<OpointType>(i).x;
			pws[3 * i + 1] = opoints.at<OpointType>(i).y;
			pws[3 * i + 2] = opoints.at<OpointType>(i).z;

			us[2 * i] = ipoints.at<IpointType>(i).x;
			us[2 * i + 1] = ipoints.at<IpointType>(i).y;
		}
	}

	/**
	 * @brief Compute the reprojection error using the estimated Rotation matrix
	 * and Translation Vector
	 * @param[in] R Rotation matrix
	 * @param[in] t Trnaslation Vector
	 * @return  Linear least squares error in pixels
	 */
	double reprojection_error(const double R[3][3], const double t[3]);

	/**
	 * @brief Function to select 4 control points
	 */
	void choose_control_points();

	/**
	 * @brief Function to comput @alphas
	 */
	void compute_alphas();

	/**
	 * @brief Function to compute Maucaulay matrix M
	 * @param[out] M Maucaulay matrix
	 * @param[in] row Internal member
	 * @param[in] alphas Internal member
	 * @param[in] u Image pixel x co-ordinate
	 * @param[in] v Image pixel y co-ordinate
	 */
	void fill_M(
		cv::Mat* M, const int row, const double* alphas, const double u,
		const double v);

	/**
	 * @brief Compute the control points
	 * @param[in] betas Internal member variable
	 * @param[in] ut Internal member variable
	 */
	void compute_ccs(const double* betas, const double* ut);

	/**
	 * @brief Compute object points based on control points
	 */
	void compute_pcs();

	/**
	 * @brief Internal member function
	 */
	void solve_for_sign();

	/**
	 * @brief Function to approximately calculate betas and focal length
	 * @param[in] Ut
	 * @param[in] Rho
	 * @param[out] betas
	 * @param[out] efs
	 */
	void find_betas_and_focal_approx_1(
		cv::Mat* Ut, cv::Mat* Rho, double* betas, double* efs);

	/**
	 * @brief Function to calculate betas and focal length (more accurate)
	 * @param[in] Ut
	 * @param[in] Rho
	 * @param[out] betas
	 * @param[out] efs
	 */
	void find_betas_and_focal_approx_2(
		cv::Mat* Ut, cv::Mat* Rho, double* betas, double* efs);

	/**
	 * @brief Function to do a QR decomposition
	 * @param[in] A Matrix to be decomposed
	 * @param[out] b
	 * @param[out] X
	 */
	void qr_solve(cv::Mat* A, cv::Mat* b, cv::Mat* X);

	/**
	 * @brief Internal function
	 * @param[in] M1
	 * @return Distance
	 */
	cv::Mat compute_constraint_distance_2param_6eq_2unk_f_unk(
		const cv::Mat& M1);

	/**
	 * @brief Internal function
	 * @param[in] M1
	 * @param[in] M2
	 * @return Distance
	 */
	cv::Mat compute_constraint_distance_3param_6eq_6unk_f_unk(
		const cv::Mat& M1, const cv::Mat& M2);

	/**
	 * @brief Get all possible solutions
	 * @param[in] betas
	 * @param[out] solutions
	 */
	void generate_all_possible_solutions_for_f_unk(
		const double betas[5], double solutions[18][3]);

	/**
	 * @brief Return the sign of the scalar
	 * @param[in] v
	 * @return True if positive else Flase
	 */
	double sign(const double v);

	/**
	 * @brief Compute the dot product between two vectors
	 * @param[in] v1
	 * @param[in] v2
	 * @return Dot product
	 */
	double dot(const double* v1, const double* v2);

	/**
	 * @brief Compute dot product in 2D with only x and y components
	 * @param[in] v1
	 * @param[in] v2
	 * @return Dot product
	 */
	double dotXY(const double* v1, const double* v2);

	/**
	 * @brief Compute the dot product using only z component
	 * @param[in] v1
	 * @param[in] v2
	 * @return Dot product
	 */
	double dotZ(const double* v1, const double* v2);

	/**
	 * @brief Compute the euclidean distance squared between two points in 3D
	 * @param[in] p1
	 * @param[in] p2
	 * @return Distance squared
	 */
	double dist2(const double* p1, const double* p2);

	/**
	 * @brief Internal fucntion
	 * @param[out] rho
	 */
	void compute_rho(double* rho);

	/**
	 * @brief Internal function
	 * @param[in] ut
	 * @param[out] l_6x12
	 */
	void compute_L_6x12(const double* ut, double* l_6x12);

	/**
	 * @brief Gauss Newton Iterative optimization
	 * @param[in] L_6x12
	 * @param[in] Rho
	 * @param[in,out] current_betas
	 * @param[out] efs
	 */
	void gauss_newton(
		const cv::Mat* L_6x12, const cv::Mat* Rho, double current_betas[4],
		double* efs);

	/**
	 * @brief Compute matrix A and vector b
	 * @param[in] l_6x12
	 * @param[in] rho
	 * @param[in] cb
	 * @param[out] A
	 * @param[out] b
	 * @param[in] f
	 */
	void compute_A_and_b_gauss_newton(
		const double* l_6x12, const double* rho, const double cb[4], cv::Mat* A,
		cv::Mat* b, double const f);

	/**
	 * @brief Function to compute the pose
	 * @param[in] ut
	 * @param[in] betas
	 * @param[out] R Rotation Matrix
	 * @param[out] t Translation VectorS
	 * @return
	 */
	double compute_R_and_t(
		const double* ut, const double* betas, double R[3][3], double t[3]);

	/**
	 * @brief Helper function to function compute_R_and_t()
	 * @param[out] R Rotaiton matrix
	 * @param[out] t Translation vector
	 */
	void estimate_R_and_t(double R[3][3], double t[3]);

	/**
	 * @brief Function to copy the pose
	 * @param[in] R_dst
	 * @param[in] t_dst
	 * @param[out] R_src
	 * @param[out] t_src
	 */
	void copy_R_and_t(
		const double R_dst[3][3], const double t_dst[3], double R_src[3][3],
		double t_src[3]);

	double uc;	//! Image center in x-direction
	double vc;	//! Image center in y-direction
	double fu;	//! Focal length in x-direction
	double fv;	//! Focal length in y-direction

	std::vector<double> pws;  //! Object points
	std::vector<double> us;	 //! Image points
	std::vector<double> alphas, pcs;  //! Internal variable
	int number_of_correspondences;	//! Number of 2d/3d correspondences

	double cws[4][3], ccs[4][3];  //! Control point variables
	int max_nr;	 //! Internal variable
	double *A1, *A2;  //! Internal variable
};

/** @}  */	// end of grouping
}  // namespace mrpt::vision::pnp
#endif	// Check for OPENCV_LIB
