////////////////////////////////////////////////////////////////////////////////
//
// 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.
////////////////////////////////////////////////////////////////////////////////
#include "Extern/ChessboardCameraTracker.hpp"
#include "Extern/utility.hpp"

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <iostream>

/**
 * It detects a chessboard inside an image and if found it returns the pose of the camera wrt the chessboard
 *
 * @param[in,out] img the original image
 * @param[out] pose the pose of the camera
 * @param[in] cam the camera
 * @param[in] boardSize the size of the chessboard to detect
 * @return true if the chessboard has been found
 */
bool ChessboardCameraTracker::process( cv::Mat &img, cv::Mat &pose, const Camera & cam, const cv::Size &boardSize )
{
    cv::Mat undistorted_img, H;

    // true if the chessboard is found
    bool found = false;

    // contains the points detected on the chessboard
    std::vector<cv::Point2f> corners;

    //******************************************************************/
    // undistort the input image. view at the end must contain the undistorted version
    // of the image.
    //******************************************************************/
    cv::undistort(img, undistorted_img, cam.matK, cam.distCoeff);
    // cv::imshow("Image View", undistorted_img);
    //******************************************************************/
    // detect the chessboard
    //******************************************************************/
    found = detectChessboard(undistorted_img, corners, boardSize);

    // cout << ( (!found ) ? ( "No " ) : ("") ) << "chessboard detected!" << endl;
    // cv::drawChessboardCorners(undistorted_img, boardSize, corners, found);

    //******************************************************************/
    // if a chessboard is found estimate the homography and rectify the image
    //******************************************************************/
    if( found )
    {

        // contains the points on the chessboard in the chessboard reference system
        std::vector<cv::Point2f> refCorners;

        //******************************************************************/
        // generate the coordinates of the points on the chessboard in the chessboard reference system
        //******************************************************************/
        calcChessboardCorners(boardSize, 25, refCorners);

        //******************************************************************/
        // estimate the homography
        // --> see findHomography
        // http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?highlight=homography#findhomography
        //******************************************************************/
        H = cv::findHomography(refCorners, corners, CV_RANSAC);


        //        cout << "H = " << H << endl << endl;
        //        cout << "corners =" << corners << endl << endl;
        //        cout << "ptsOb =" << objectPoints << endl << endl;

        //******************************************************************/
        // decompose the homography
        //******************************************************************/
        decomposeHomography(H, cam.matK, pose);
    }

    return found;
}