#include <maps/wikimap/mapspro/services/mrc/eye/lib/location/include/camera.h>

#include <maps/libs/common/include/exception.h>

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

#include <map>

#include <math.h>

namespace maps::mrc::eye {

namespace SM_A720F {

static double CAMERA_MATRIX[3][3] {
    {1550.2831676510534, 0.0, 965.3485299418829},
    {0.0, 1608.0427985096308, 562.8612217585091},
    {0.0, 0.0, 1.0},
};

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

} // SM_A720F

namespace SM_A320F {

static double CAMERA_MATRIX[3][3] {
    {1548.5018915566839, 0.0, 958.0897383016004},
    {0.0, 1606.6414025363163, 514.8638298826688},
    {0.0, 0.0, 1.0},
};

static double DISTORTION_COEFFICIENTS[5] {
    0.2865727735169787,
    -1.4149232899218065,
    0.0003994526868998917,
    0.0027783871332046953,
    2.144550035840695,
};

} // SM_A320F

namespace ZE520KL {

static double CAMERA_MATRIX[3][3] {
    {1457.4788126595836, 0.0, 940.3864712537568},
    {0.0, 1521.3745089700094, 582.3979995887518},
    {0.0, 0.0, 1.0},
};

static double DISTORTION_COEFFICIENTS[5] {
    0.34102711210660236,
    -2.9411569828975668,
    4.4234441167407114e-05,
    0.002258614752318472,
    10.806146078384451
};

} // ZE520KL

namespace {

const static std::unordered_map<std::string, CameraParameters> cameraParametersByModel {
    {
        "SM-A320F",
        CameraParameters {
            cv::Mat(3, 3, CV_64F, SM_A320F::CAMERA_MATRIX),
            cv::Mat(5, 1, CV_64F, SM_A320F::DISTORTION_COEFFICIENTS)
        }
    },
    {
        "SM-A720F",
        CameraParameters {
            cv::Mat(3, 3, CV_64F, SM_A720F::CAMERA_MATRIX),
            cv::Mat(5, 1, CV_64F, SM_A720F::DISTORTION_COEFFICIENTS)
        }
    },
    {
        "ZE520KL",
        CameraParameters {
            cv::Mat(3, 3, CV_64F, ZE520KL::CAMERA_MATRIX),
            cv::Mat(5, 1, CV_64F, ZE520KL::DISTORTION_COEFFICIENTS)
        }
    },
};


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

bool isHorizontal(const common::ImageOrientation& orientation)
{
    return orientation.rotation() == Rotation::CW_0 ||
        orientation.rotation() == Rotation::CW_180;
}

// Compute vertical FOV angle out of horizontal FOV angle for a given
// rectangle size in pixels which are assumed to be perfect squares. It is
// also assumed the principal point is exactly in the centre of this
// rectangle. I.e. the resulting vertical FOV angle is computed for an ideal
// pin-hole camera projection onto a rectangle of a given size which is
// coplanar with the projective plain.
//
// Also see "Detailed Description" chapter at
// https://docs.opencv.org/4.5.2/d9/d0c/group__calib3d.html
geolib3::Radians computeVerticalFOV(const common::ImageOrientation& orientation,
                                       const common::Size& size,
                                       const geolib3::Radians& horizontalFOV)
{
    // The computation is done with a focal length equal to 1.0.
    const double aspectRatio = isHorizontal(orientation)
                                   ? (double)size.width / (double)size.height
                                   : (double)size.height / (double)size.width;
    const double relativeHalfVerticalFOV = geolib3::tan(horizontalFOV / 2);
    const double relativeHalfHeight = relativeHalfVerticalFOV / aspectRatio;
    return geolib3::Radians(std::atan(relativeHalfHeight) * 2);
}

CameraParameters generateIdealCameraParameters(
    const common::ImageOrientation& orientation,
    common::Size size,
    geolib3::Radians cameraHeightAngle)
{
    CameraParameters parameters {
        cv::Mat::zeros(3, 3, CV_64F),
        cv::Mat::zeros(5, 1, CV_64F)
    };

    auto& matrix = parameters.cameraMatrix;
    matrix.at<double>(2, 2) = 1.0;

    // set focal length
    const double focal_length = isHorizontal(orientation)
        ? (size.height / 2)  / geolib3::tan(cameraHeightAngle / 2)
        : (size.width / 2)  / geolib3::tan(cameraHeightAngle / 2);

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

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

    return parameters;
}

const geolib3::Radians DEFAULT_CAMERA_HEIGHT_ANGLE = geolib3::toRadians(38_deg);
const common::Size DEFAULT_HORIZONTAL_IMAGE_SIZE {1920, 1080};
const common::Size DEFAULT_VERTICAL_IMAGE_SIZE {1080, 1920};

bool isDefaultSize(const common::ImageOrientation& orientation,
                   const common::Size& size)
{
    return isHorizontal(orientation)
        ? size == DEFAULT_HORIZONTAL_IMAGE_SIZE
        : size == DEFAULT_VERTICAL_IMAGE_SIZE;
}

CameraParameters generateCameraParameters(
    CameraParameters parameters,
    const common::ImageOrientation& orientation,
    const common::Size& size)
{
    if (orientation.rotation() == Rotation::CW_90) {
        cv::Mat matrix = parameters.cameraMatrix.clone();

        matrix.at<double>(0, 0) = parameters.cameraMatrix.at<double>(1, 1);
        matrix.at<double>(1, 1) = parameters.cameraMatrix.at<double>(0, 0);

        matrix.at<double>(0, 2) = size.width - parameters.cameraMatrix.at<double>(1, 2);
        matrix.at<double>(1, 2) = parameters.cameraMatrix.at<double>(0, 2);

        parameters.cameraMatrix = matrix;
    } else if (orientation.rotation() == Rotation::CW_180) {
        auto& matrix = parameters.cameraMatrix;

        matrix.at<double>(0, 2) = size.width - matrix.at<double>(0, 2);
        matrix.at<double>(1, 2) = size.height - matrix.at<double>(1, 2);
    } else if (orientation.rotation() == Rotation::CW_270) {
        cv::Mat matrix = parameters.cameraMatrix.clone();

        matrix.at<double>(0, 0) = parameters.cameraMatrix.at<double>(1, 1);
        matrix.at<double>(1, 1) = parameters.cameraMatrix.at<double>(0, 0);

        matrix.at<double>(0, 2) = parameters.cameraMatrix.at<double>(1, 2);
        matrix.at<double>(1, 2) = size.height - parameters.cameraMatrix.at<double>(0, 2);

        parameters.cameraMatrix = matrix;
    }

    return parameters;

}

CameraParameters getCameraParameters(
        const std::string& model,
        const common::ImageOrientation& orientation,
        const common::Size& size)
{
    ASSERT(!orientation.horizontalFlip());

    const auto it = cameraParametersByModel.find(model);

    if (!isDefaultSize(orientation, size) || it == cameraParametersByModel.end()) {
        return generateIdealCameraParameters(orientation, size,
                                             DEFAULT_CAMERA_HEIGHT_ANGLE);
    }

    return generateCameraParameters(it->second, orientation, size);
}

} // namespace

CameraParameters getCameraParameters(const db::eye::DeviceAttrs& deviceAttrs,
                                     const common::ImageOrientation& orientation,
                                     const common::Size& size)
{
    switch (deviceAttrs.source()) {
        case db::eye::FrameSource::Mrc:
            return getCameraParameters(
                deviceAttrs.mrc().model.value_or(""), orientation, size);

        case db::eye::FrameSource::Panorama:
            const auto verticalFOV = computeVerticalFOV(
                orientation, size,
                geolib3::toRadians(deviceAttrs.panorama().horizontalFOV));

            return generateIdealCameraParameters(orientation, size, verticalFOV);
    }
}

} // namespace maps::mrc::eye
