#include "rodrigues.h"

#include <opencv2/opencv.hpp>

#include "cmath"

namespace maps::mrc::pos_improvment {

std::vector<double> toRodrigues(const CameraOrientation& cameraOrientation)
{
    const auto& newX = cameraOrientation.cameraRightDirection;
    const auto& newY = cameraOrientation.cameraFrontDirection;
    const auto& newZ = cameraOrientation.cameraUpDirection;

    std::vector<double> axes {
        newX.x(), newY.x(), newZ.x(),
        newX.y(), newY.y(), newZ.y(),
        newX.z(), newY.z(), newZ.z()};
    cv::Mat axesMat(3, 3, cv::DataType<double>::type, &axes[0]);

    cv::Mat anglesMat;
    cv::Rodrigues(axesMat, anglesMat);

    return {anglesMat.begin<double>(), anglesMat.end<double>()};
};

CameraOrientation fromRodrigues(std::vector<double> rodrigues)
{
    cv::Mat anglesMat(1, 3, cv::DataType<double>::type, &rodrigues[0]);

    cv::Mat axesMat;
    cv::Rodrigues(anglesMat, axesMat);

    geolib3::Vector3 newX {axesMat.at<double>(0, 0), axesMat.at<double>(1, 0), axesMat.at<double>(2, 0)};
    geolib3::Vector3 newY {axesMat.at<double>(0, 1), axesMat.at<double>(1, 1), axesMat.at<double>(2, 1)};
    geolib3::Vector3 newZ {axesMat.at<double>(0, 2), axesMat.at<double>(1, 2), axesMat.at<double>(2, 2)};

    return CameraOrientation{
        UnitVector3(newX),
        UnitVector3(newY),
        UnitVector3(newZ)};
}

} // namespace maps::mrc::pos_improvment
