////////////////////////////////////////////////////////////////////////////////
//
// 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 <boost/program_options.hpp>

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

#include <fstream>
#include <iostream>

// parse the input command line arguments
// Return a pair of boolean
//    - the first one is true if the execution must go on
//    - the second one is false if there was a problem (main must return non-0 value)
std::pair<bool, bool> parseArgs(int argc, char**argv, cv::Size &boardSize, std::string &inputFilename, std::string& calibFilename, bool& displayImage, std::string& outputFilename)
{
    namespace po = boost::program_options;

    try
    {
        po::options_description desc("Allowed options");

        desc.add_options()
            ("help", "procude help message")
            ("calib,c", po::value<std::string>(&calibFilename)->required(), "path to the calibration file")
            ("width,w", po::value<int>(&boardSize.width)->required(), "number of corners per line in the chessboard")
            ("height,h", po::value<int>(&boardSize.height)->required(), "number of corners per coloumn in the chessboard")
            ("image,i", po::value<std::string>(&inputFilename)->required(), "image to calibrate")
            ("output,o", po::value<std::string>(&outputFilename), "path to the output file (stdout if not specified)")
            ("show-image,s", "show the reference system in augmented reality on the picture");

        po::positional_options_description p;
        p.add("image", -1);

        po::variables_map vm;
        po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);

        displayImage = vm.count("show-image");

        if (vm.count("help"))
        {
            std::cout << desc << "\n";
            return {false, true};
        }
        po::notify(vm);

    }
    catch (std::exception const& e)
    {
        std::cerr << "Unable to parse : " << e.what() << std::endl;
        return {false, false};
    }

    return {true, true};

}


int main( int argc, char** argv )
{
    std::string calibFilename, outputFilename;
    bool displayImage;


    cv::Mat view;            // it will contain the original image loaded from file
    Camera cam;

    ChessboardCameraTracker tracker;
    cv::Mat pose;

    // it will contain the size in terms of corners (width X height) of the chessboard
    cv::Size boardSize;

    // it will contains the filename of the image file
    std::string inputFilename;

    // Parse the arugments
    auto pair = parseArgs(argc, argv, boardSize, inputFilename, calibFilename, displayImage, outputFilename);

    if (!pair.second)
    {
        std::cerr << "Aborting..." << std::endl;
        return EXIT_FAILURE;
    }

    if (!pair.first)
    {
        return EXIT_SUCCESS;
    }

    if (!cam.init(calibFilename))
    {
        return EXIT_FAILURE;
    }
    view = cv::imread(inputFilename, CV_LOAD_IMAGE_COLOR);

    if(tracker.process(view, pose, cam, boardSize))
    {
        std::cout << "Process worked correctly !"<<  std::endl;
        if (displayImage)
        {
            const std::string WINDOW_NAME = "External calibration";
            cv::namedWindow(WINDOW_NAME, CV_WINDOW_AUTOSIZE );
            drawReferenceSystem(view,cam,pose,1,200,false );
            cv::imshow(WINDOW_NAME, view);
            cv::waitKey( -1 );
        }
    }
    else
    {
        std::cout << "Unable to process" << std::endl;
        return EXIT_FAILURE;
    }

    if (outputFilename == "")
    {
        std::cout << "External matrix is \n" << pose << std::endl;
    }
    else
    {
        std::ofstream file{outputFilename};
        file << pose << std::endl;
    }

    return EXIT_SUCCESS;
}