#include <library/cpp/testing/unittest/env.h>
#include <library/cpp/testing/unittest/registar.h>
#include <library/cpp/testing/gmock_in_unittest/gmock.h>

#include <maps/libs/geolib/include/units.h>
#include <maps/libs/geolib/include/units_literals.h>

#include <maps/wikimap/mapspro/services/mrc/libs/camera/include/camera_parameters.h>

#include <opencv2/opencv.hpp>


namespace maps {
namespace mrc {

namespace tests {

void baseMatrixCheck(const cv::Mat& matrix)
{
    EXPECT_EQ(matrix.size(), cv::Size(3, 3));

    EXPECT_EQ(matrix.at<double>(2, 2), 1);

    EXPECT_EQ(matrix.at<double>(0, 1), 0);
    EXPECT_EQ(matrix.at<double>(1, 0), 0);
    EXPECT_EQ(matrix.at<double>(2, 0), 0);
    EXPECT_EQ(matrix.at<double>(2, 1), 0);
}

template<typename T>
bool equal(const cv::Mat& lhs, const cv::Mat& rhs)
{
    return (lhs.size() == rhs.size())
        && std::equal(
            lhs.begin<T>(), lhs.end<T>(),
            rhs.begin<T>()
        );
}

const cv::Mat NO_DISTORTION = cv::Mat::zeros(5, 1, CV_64F);

using namespace geolib3::literals;
using Rotation = common::Rotation;

const geolib3::Radians DEFAULT_CAMERA_HEIGHT_ANGLE = geolib3::toRadians(38_deg);

TEST(camera_parameters_tests, horizontal_default_camera)
{
    const cv::Size size {1200, 800};

    const CameraParameters parameters = getCameraParameters(
        "unknown model",
        common::ImageOrientation{false, Rotation::CW_0},
        size
    );

    const cv::Mat matrix = parameters.cameraMatrix;

    baseMatrixCheck(matrix);

    EXPECT_EQ(matrix.at<double>(0, 2), size.width / 2);
    EXPECT_EQ(matrix.at<double>(1, 2), size.height / 2);

    EXPECT_EQ(matrix.at<double>(0, 0), matrix.at<double>(1, 1));

    EXPECT_EQ(
        (size.height / 2) / matrix.at<double>(0, 0),
        geolib3::tan(DEFAULT_CAMERA_HEIGHT_ANGLE / 2)
    );

    EXPECT_TRUE(equal<double>(NO_DISTORTION, parameters.distortionCoefficients));
}


TEST(camera_parameters_tests, vertical_default_camera)
{
    const cv::Size size {800, 1200};

    const CameraParameters parameters = getCameraParameters(
        "unknown model",
        common::ImageOrientation{false, Rotation::CW_90},
        size
    );

    const cv::Mat matrix = parameters.cameraMatrix;

    baseMatrixCheck(matrix);

    EXPECT_EQ(matrix.at<double>(0, 2), size.width / 2);
    EXPECT_EQ(matrix.at<double>(1, 2), size.height / 2);

    EXPECT_EQ(matrix.at<double>(0, 0), matrix.at<double>(1, 1));

    EXPECT_EQ(
        (size.width / 2) / matrix.at<double>(0, 0),
        geolib3::tan(DEFAULT_CAMERA_HEIGHT_ANGLE / 2)
    );

    EXPECT_TRUE(equal<double>(NO_DISTORTION, parameters.distortionCoefficients));
}


TEST(camera_parameters_tests, vertical_calibrated_camera)
{
    const cv::Size size {1080, 1920};

    const CameraParameters parameters = getCameraParameters(
        "SM-A720F",
        common::ImageOrientation{false, Rotation::CW_90},
        size
    );

    static double CAMERA_MATRIX_DATA[3][3] {
        {1608.0427985096308, 0.0, size.width - 562.8612217585091},
        {0.0, 1550.2831676510534, 965.3485299418829},
        {0.0, 0.0, 1.0},
    };

    static const cv::Mat CAMERA_MATRIX(3, 3, CV_64F, CAMERA_MATRIX_DATA);

    static double DISTORTION_COEFFICIENTS_DATA[5] {
        0.3563727034115357,
        -1.0098845221501338,
        0.008230623224778326,
        0.00291693863038959,
        -1.3848021729577955,
    };

    static const cv::Mat DISTORTION_COEFFICIENTS(5, 1, CV_64F, DISTORTION_COEFFICIENTS_DATA);

    EXPECT_TRUE(equal<double>(CAMERA_MATRIX, parameters.cameraMatrix));
    EXPECT_TRUE(equal<double>(DISTORTION_COEFFICIENTS, parameters.distortionCoefficients));
}

} //namespace tests

} //namespace mrc
} //namespace maps
