////////////////////////////////////////////////////////////////////////////////
//
// Paella
// Copyright (C) 2015 - Thomas FORGIONE, Emilie JALRAS, Marion LENFANT, Thierry MALON, Amandine PAILLOUX
// Authors :
//     Thomas FORGIONE
//     Emilie JALRAS
//     Marion LENFANT
//     Thierry MALON
//     Amandine PAILLOUX
//     Simone GASPARINI
//
// This file is part of the project Paella
// This software is provided 'as-is', without any express or implied warranty.
// In no event will the authors be held liable for any damages arising from the use of this software.
//
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it freely,
// subject to the following restrictions:
//
// 1. The origin of this software must not be misrepresented;
//    you must not claim that you wrote the original software.
//    If you use this software in a product, an acknowledgment
//    in the product documentation would be appreciated but is not required.
//
// 2. Altered source versions must be plainly marked as such,
//    and must not be misrepresented as being the original software.
//
// 3. This notice may not be removed or altered from any source distribution.
////////////////////////////////////////////////////////////////////////////////
#ifndef _UTILITY_HPP_
#define _UTILITY_HPP_

#include "Camera.hpp"

#include <opencv2/core/core.hpp>

#include <iostream>

#define DEBUGGING 1

#if DEBUGGING
#define PRINTVAR( a ) std::cout << #a << " = " << a << std::endl << std::endl;
#else
#define PRINTVAR( a )
#endif

/**
 * \ingroup calibration
 * Detect a chessboard in a given image
 *
 * @param[in] rgbimage The rgb image to process
 * @param[out] pointbuf the set of 2D image corner detected on the chessboard
 * @param[in] boardSize the size of the board in terms of corners (width X height)
 * @return true if the chessboard is detected inside the image, false otherwise
 */
bool detectChessboard( const cv::Mat &rgbimage, std::vector<cv::Point2f> &pointbuf, const cv::Size &boardSize);

/**
 * \ingroup calibration
 * Decompose the homography into its components R and t
 *
 * @param[in] H The homography H = [r1 r2 t]
 * @param[in] matK The 3x3 calibration matrix K
 * @param[out] poseMat the 3x4 pose matrix [R t]
 */
void decomposeHomography( const cv::Mat &H, const cv::Mat& matK, cv::Mat& poseMat );

/**
 *
 * \ingroup calibration
 * @param[in,out] rgbimage The image on which to draw the reference system
 * @param[in] cam The camera
 * @param[in] projMat The projection matrix of the camera
 * @param[in] thickness The thickness of the line
 * @param[in] scale A scale factor for the unit vectors to draw
 * @param[in] alreadyUndistorted A boolean value that tells if the input image rgbimage is already undistorted or we are working on a distorted image
 */
void drawReferenceSystem( cv::Mat &rgbimage, const Camera& cam, const cv::Mat &projMat, const int &thickness, const double &scale, const bool alreadyUndistorted = true );

/**
 * \ingroup calibration
 * Wrapper around the original opencv's projectPoints
 *
 * @param[in] objectPoints the 3D points
 * @param[in] poseMat the pose matrix
 * @param[in] cameraMatrix the calibration matrix
 * @param[in] distCoeffs the distortion coefficients
 * @param[out] imagePoints the projected points
 */
void myProjectPoints( cv::InputArray objectPoints, const cv::Mat &poseMat, cv::InputArray cameraMatrix, cv::InputArray distCoeffs, cv::OutputArray imagePoints);

/**
 * \ingroup calibration
 * Wrapper around the original opencv's solvePnPRansac
 *
 * @param[in] objectPoints the 3D points
 * @param[in] imagePoints the image points
 * @param[in] cameraMatrix the calibration matrix
 * @param[in] distCoeffs the distortion coefficients
 * @param[out] poseMat the pose matrix
 * @param[out] inliers the list of indices of the inliers points
 */
void mySolvePnPRansac(cv::InputArray objectPoints, cv::InputArray imagePoints, cv::InputArray cameraMatrix, cv::InputArray distCoeffs, cv::Mat &poseMat, cv::OutputArray inliers=cv::noArray());

/**
 * \ingroup calibration
 * Generate the set of 3D points of a chessboard
 *
 * @param[in] boardSize the size of the board in terms of corners (width X height)
 * @param[in] squareSize the size in mm of the each square of the chessboard
 * @param[out] corners the set of 2D points on the chessboard
 */
void calcChessboardCorners( const cv::Size &boardSize, const float &squareSize, std::vector<cv::Point2f>& corners);


/**
 * \ingroup calibration
 * Generate the set of 3D points of a chessboard
 *
 * @param[in] boardSize the size of the board in terms of corners (width X height)
 * @param[in] squareSize the size in mm of the each square of the chessboard
 * @param[out] corners the set of 3D points on the chessboard
 */
void calcChessboardCorners3D( const cv::Size &boardSize, const float &squareSize, std::vector<cv::Point3f>& corners );

/**
 * \ingroup calibration
 * Filter a generic vector against a list of index of the elements to be deleted,
 *
 * @param[in,out] inout the vector to filter
 * @param[in] idx list of indices of the element to remove
 */
template<typename T>
void filterVector( std::vector<T> &inout, const std::vector<int> &idx )
{
    std::vector<T> temp;
    temp.reserve( idx.size() );

    for( size_t i = 0; i < idx.size(); ++i )
    {
        assert( idx[i] < inout.size() );
        temp.push_back( inout[ idx[i] ] );
    }

    inout.clear();  //necessary??
    inout = temp;
}

#endif /*_UTILITY_HPP_*/