/************************************************************************
 *
 * Copyright (C) 2019 IRCAD France
 * Copyright (C) 2019 IHU Strasbourg
 *
 * This file is part of Sight.
 *
 * Sight is free software: you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Sight is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with Sight. If not, see <https://www.gnu.org/licenses/>.
 *
 ***********************************************************************/

#include "HelperTest.hpp"

#include <openvslamIO/Helper.hpp>

#include <arData/Camera.hpp>

#include <fwTools/System.hpp>

#include <openvslam/camera/perspective.h>
#include <openvslam/system.h>

#include <spdlog/spdlog.h>

CPPUNIT_TEST_SUITE_REGISTRATION( ::openvslamIO::ut::HelperTest );

namespace openvslamIO
{
namespace ut
{

//-----------------------------------------------------------------------------

void HelperTest::setUp()
{

}

//-----------------------------------------------------------------------------

void HelperTest::tearDown()
{

}

//-----------------------------------------------------------------------------

void HelperTest::toSight()
{
    const ::openvslam::camera::perspective oVSlamCam =
        ::openvslam::camera::perspective("Dummy SLAM camera", ::openvslam::camera::setup_type_t::Monocular,
                                         ::openvslam::camera::color_order_t::RGB,
                                         1920, 1080, 60,
                                         400, 401, 200, 201, 0.0, 0.1, 0.2, 0.3, 0.4);

    const ::arData::Camera::sptr cam = ::openvslamIO::Helper::toSight(oVSlamCam);

    compareCam(cam, oVSlamCam, true);

}

//-----------------------------------------------------------------------------

void HelperTest::fromSight()
{
    //Create a dummy ::arData::Camera;
    ::arData::Camera::sptr cam = ::arData::Camera::New();

    cam->setCameraID("Dummy Test Camera");
    cam->setWidth(1920);
    cam->setWidth(1080);
    cam->setFx(400);
    cam->setFy(401);
    cam->setCx(200);
    cam->setCy(201);
    cam->setDistortionCoefficient(0.0, 0.1, 0.2, 0.3, 0.4);
    cam->setMaximumFrameRate(60.0);

    const auto oVSlamCam = ::openvslamIO::Helper::fromSight(cam);

    // Compare values.
    compareCam(cam, oVSlamCam, false);

}

//-----------------------------------------------------------------------------

void HelperTest::createConfig()
{
    //Create a dummy ::arData::Camera;
    ::arData::Camera::sptr cam = ::arData::Camera::New();

    cam->setCameraID("Dummy Test Camera");
    cam->setWidth(1920);
    cam->setWidth(1080);
    cam->setFx(400);
    cam->setFy(401);
    cam->setCx(200);
    cam->setCy(201);
    cam->setDistortionCoefficient(0.0, 0.1, 0.2, 0.3, 0.4);
    cam->setMaximumFrameRate(60.0);

    ::openvslamIO::OrbParams orbParam;
    orbParam.maxNumKeyPts = 8000;
    orbParam.scaleFactor  = 1.2f;
    orbParam.iniFastThr   = 2;
    orbParam.minFastThr   = 1;
    orbParam.numLevels    = 8;

    const auto config = ::openvslamIO::Helper::createMonocularConfig(cam, orbParam);

    // We know that the camera is perspective.
    ::openvslam::camera::perspective* camera = dynamic_cast< ::openvslam::camera::perspective* >(config->camera_);

    ::openvslam::feature::orb_params orbParamFromConfig = config->orb_params_;

    compareCam(cam, *camera, true);

    CPPUNIT_ASSERT_EQUAL(orbParamFromConfig.max_num_keypts_, orbParam.maxNumKeyPts);
    CPPUNIT_ASSERT_EQUAL(orbParamFromConfig.num_levels_, orbParam.numLevels);
    CPPUNIT_ASSERT_EQUAL(orbParamFromConfig.ini_fast_thr_, orbParam.iniFastThr);
    CPPUNIT_ASSERT_EQUAL(orbParamFromConfig.min_fast_thr, orbParam.minFastThr);
    CPPUNIT_ASSERT_EQUAL(orbParamFromConfig.scale_factor_, orbParam.scaleFactor);

}

//-----------------------------------------------------------------------------

void HelperTest::writeReadConfig()
{
    //Create a dummy ::arData::Camera;
    ::arData::Camera::sptr cam = ::arData::Camera::New();

    cam->setCameraID("Dummy Test Camera");
    cam->setWidth(1920);
    cam->setWidth(1080);
    cam->setFx(400);
    cam->setFy(401);
    cam->setCx(200);
    cam->setCy(201);
    cam->setDistortionCoefficient(0.0, 0.1, 0.2, 0.3, 0.4);
    cam->setMaximumFrameRate(60.0);

    ::openvslamIO::OrbParams orbParam;
    orbParam.maxNumKeyPts = 8000;
    orbParam.scaleFactor  = 1.2f;
    orbParam.iniFastThr   = 2;
    orbParam.minFastThr   = 1;
    orbParam.numLevels    = 8;

    ::openvslamIO::InitParams initParams;
    initParams.reprojErrThr          = 10.f;
    initParams.scalingFactor         = 2.f;
    initParams.parallaxDegThr        = 3.f;
    initParams.numBAIterations       = 101;
    initParams.numRansacIterations   = 9;
    initParams.minNumTriangulatedPts = 45;

    const auto config = ::openvslamIO::Helper::createMonocularConfig(cam, orbParam, initParams);

    const std::filesystem::path tmp = ::fwTools::System::getTemporaryFolder();

    CPPUNIT_ASSERT_NO_THROW(::openvslamIO::Helper::writeOpenvslamConfig(config->yaml_node_,
                                                                        tmp.string() + "/test.yaml"));

    const auto config2 = ::openvslamIO::Helper::readOpenvslamConfig(tmp.string() + "/test.yaml");
    CPPUNIT_ASSERT(config2 != nullptr);

    const auto orb = config2->orb_params_;
    ::openvslam::camera::perspective* camera = dynamic_cast< ::openvslam::camera::perspective* >(config2->camera_);

    CPPUNIT_ASSERT_EQUAL(orbParam.maxNumKeyPts, orb.max_num_keypts_);
    CPPUNIT_ASSERT_EQUAL(orbParam.numLevels, orb.num_levels_ );
    CPPUNIT_ASSERT_EQUAL(orbParam.iniFastThr, orb.ini_fast_thr_ );
    CPPUNIT_ASSERT_EQUAL(orbParam.minFastThr, orb.min_fast_thr );
    CPPUNIT_ASSERT_EQUAL(orbParam.scaleFactor, orb.scale_factor_ );

    compareCam(cam, *camera, false);

    // Since config doesn't export initializer parameters we need to parse the node to check if parameters are correct.

    const auto node = config2->yaml_node_;

    CPPUNIT_ASSERT_DOUBLES_EQUAL(static_cast<double>(initParams.reprojErrThr),
                                 node["Initializer.reprojection_error_threshold"].as<double>(), 10e-7);
    CPPUNIT_ASSERT_DOUBLES_EQUAL(static_cast<double>(initParams.scalingFactor),
                                 node["Initializer.scaling_factor"].as<double>(),  10e-7);
    CPPUNIT_ASSERT_DOUBLES_EQUAL(static_cast<double>(initParams.parallaxDegThr),
                                 node["Initializer.parallax_deg_threshold"].as<double>(), 10e-7);
    CPPUNIT_ASSERT_EQUAL(initParams.numBAIterations, node["Initializer.num_ba_iterations"].as<unsigned int>());
    CPPUNIT_ASSERT_EQUAL(initParams.numRansacIterations, node["Initializer.num_ransac_iterations"].as<unsigned int>());
    CPPUNIT_ASSERT_EQUAL(initParams.minNumTriangulatedPts,
                         node["Initializer.num_min_triangulated_pts"].as<unsigned int>());

}

//-----------------------------------------------------------------------------

void HelperTest::compareCam(const arData::Camera::csptr _sightCam, const openvslam::camera::perspective& _ovsCam,
                            bool _sightExpected)
{
    if(_sightExpected)
    {
        CPPUNIT_ASSERT_EQUAL(_sightCam->getCameraID(), _ovsCam.name_);
        CPPUNIT_ASSERT_EQUAL(_sightCam->getWidth(), static_cast<size_t>(_ovsCam.cols_));
        CPPUNIT_ASSERT_EQUAL(_sightCam->getHeight(), static_cast<size_t>(_ovsCam.rows_));
        CPPUNIT_ASSERT_EQUAL(_sightCam->getFx(), _ovsCam.fx_);
        CPPUNIT_ASSERT_EQUAL(_sightCam->getFy(), _ovsCam.fy_);
        CPPUNIT_ASSERT_EQUAL(_sightCam->getCx(), _ovsCam.cx_);
        CPPUNIT_ASSERT_EQUAL(_sightCam->getCy(), _ovsCam.cy_);

        const auto dist = _sightCam->getDistortionCoefficient();

        CPPUNIT_ASSERT_DOUBLES_EQUAL(dist[0], _ovsCam.k1_, 10e-8);
        CPPUNIT_ASSERT_DOUBLES_EQUAL(dist[1], _ovsCam.k2_, 10e-8);
        CPPUNIT_ASSERT_DOUBLES_EQUAL(dist[2], _ovsCam.p1_, 10e-8);
        CPPUNIT_ASSERT_DOUBLES_EQUAL(dist[3], _ovsCam.p2_, 10e-8);
        CPPUNIT_ASSERT_DOUBLES_EQUAL(dist[4], _ovsCam.k3_, 10e-8);
    }
    else
    {
        CPPUNIT_ASSERT_EQUAL(_ovsCam.name_, _sightCam->getCameraID());
        CPPUNIT_ASSERT_EQUAL(static_cast<size_t>(_ovsCam.cols_), _sightCam->getWidth());
        CPPUNIT_ASSERT_EQUAL(static_cast<size_t>(_ovsCam.rows_), _sightCam->getHeight());
        CPPUNIT_ASSERT_EQUAL(_ovsCam.fx_, _sightCam->getFx());
        CPPUNIT_ASSERT_EQUAL(_ovsCam.fy_, _sightCam->getFy());
        CPPUNIT_ASSERT_EQUAL(_ovsCam.cx_, _sightCam->getCx());
        CPPUNIT_ASSERT_EQUAL(_ovsCam.cy_, _sightCam->getCy());

        const auto dist = _sightCam->getDistortionCoefficient();

        CPPUNIT_ASSERT_DOUBLES_EQUAL(_ovsCam.k1_, dist[0], 10e-8);
        CPPUNIT_ASSERT_DOUBLES_EQUAL(_ovsCam.k2_, dist[1], 10e-8);
        CPPUNIT_ASSERT_DOUBLES_EQUAL(_ovsCam.p1_, dist[2], 10e-8);
        CPPUNIT_ASSERT_DOUBLES_EQUAL(_ovsCam.p2_, dist[3], 10e-8);
        CPPUNIT_ASSERT_DOUBLES_EQUAL(_ovsCam.k3_, dist[4], 10e-8);
    }
}

//-----------------------------------------------------------------------------
}
}
