#include <iostream>
#include <Eigen/Dense>
#include <iostream>
using std::cout;
using std::endl;

int main() {
    // Generate 20 random 2D points (source points)
    Eigen::MatrixXd src_points(2, 20);
    src_points = Eigen::MatrixXd::Random(2, 20);

    // Define a known rotation matrix R and translation vector t
    double theta = M_PI / 4;  // 45 degrees rotation
    Eigen::Matrix2d R;
    R << std::cos(theta), -std::sin(theta),
         std::sin(theta),  std::cos(theta);
    Eigen::Vector2d t(1.0, 2.0);

    // Apply the transformation to generate the destination points
    Eigen::MatrixXd dst_points = (R * src_points).colwise() + t;

    // Use Eigen's Umeyama function to estimate the transformation
    Eigen::Matrix3d T = Eigen::umeyama(src_points, dst_points, true);

    // Print the estimated transformation matrix
    std::cout << "Estimated transformation matrix:\n" << T << std::endl;

    // Apply the resulting transformation to the source points
    Eigen::MatrixXd src_points_hom(3, 20);
    src_points_hom.topRows(2) = src_points;
    src_points_hom.row(2) = Eigen::RowVectorXd::Ones(20);

    Eigen::MatrixXd aligned_points = (T * src_points_hom).topRows(2);

    // Print the original, transformed, and recovered points
    std::cout << "Original Source Points:\n" << src_points << std::endl;
    std::cout << "Transformed Destination Points:\n" << dst_points << std::endl;
    std::cout << "Recovered Aligned Points:\n" << aligned_points << std::endl;

    // Calculate the difference between the destination points and the aligned points
    double difference = (dst_points - aligned_points).norm();
    std::cout << "\nDifference between destination and aligned points: " << difference << std::endl;

    return 0;
}