#include "cylinder_face.hpp"
#include <Eigen/Dense>
#include <cassert>
#include <iostream>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

// 工具函数：打印分隔线
void print_separator() {
    std::cout << "\n" << std::string(60, '-') << "\n" << std::endl;
}

// 工具函数：打印测试标题
void print_test_title(const std::string& title) {
    std::cout << "\033[1;36m"; // 浅蓝色
    std::cout << title << "\033[0m" << std::endl;
}

// 工具函数：打印哈希值
void print_hash(size_t h, const std::string& label) {
    std::cout << "   " << label << " Hash = " << h << std::endl;
}

// 工具函数：断言并输出通过/失败
void assert_equal_hashes(size_t h1, size_t h2, const std::string& msg) {
    if (h1 == h2) {
        std::cout << "   \033[32mPASSED\033[0m: " << msg << std::endl;
    } else {
        std::cout << "   \033[31mFAILED\033[0m: " << msg << std::endl;
        std::cout << "      Hash1: " << h1 << "\n      Hash2: " << h2 << std::endl;
        assert(false);
    }
}

void test_cylinder_hasher_equivalence() {
    print_test_title("Test 1: Same Cylinder, Different Scale and Offset");
    std::cout 
        << "Purpose: Verify hash is invariant under:\n"
        << "   - Uniform scaling in xy-plane\n"
        << "   - Translation along z-axis\n"
        << "   - Combined transformations\n"
        << "Cylinder 1: Identity (unit radius, axis = Z)\n"
        << "Cylinder 2: Scaled by 2 in xy, rotated -30 degree around Z, shifted by (0,0,5)\n"
        << "Expected: Same hash\n";

    using namespace internal;
    detail::hasher<cylinder_paired_model_matrix> hasher;

    // Cylinder 1: Identity
    auto mat1 = std::make_unique<paired_model_matrix>();
    mat1->world_to_local = Eigen::Affine3d::Identity();

    // Cylinder 2: Scaled, rotated, translated
    auto mat2 = std::make_unique<paired_model_matrix>();
    Eigen::Affine3d t = Eigen::Affine3d::Identity();
    t.linear() = Eigen::AngleAxisd(-M_PI/6, Eigen::Vector3d::UnitZ()).toRotationMatrix() 
               * Eigen::DiagonalMatrix<double,3>(2.0, 2.0, 1.0);
    t.translation() = Eigen::Vector3d(0, 0, 5); // only z-translation matters
    mat2->world_to_local = t;

    cylinder_paired_model_matrix block1, block2;
    block1.data = mat1.get();
    block2.data = mat2.get();

    size_t h1 = hasher(block1);
    size_t h2 = hasher(block2);

    print_hash(h1, "Cylinder 1");
    print_hash(h2, "Cylinder 2");
    assert_equal_hashes(h1, h2, "Scaled/rotated/shifted cylinder should hash the same");
    print_separator();
}

void test_cylinder_hasher_rotation_invariance() {
    print_test_title("Test 2: Invariance Under Local Frame Rotation");
    std::cout 
        << "Purpose: Verify hash is invariant under rotation of local x/y axes\n"
        << "   (around cylinder axis)\n"
        << "Cylinder 1: Identity\n"
        << "Cylinder 2: Rotated by 90 degree around Z-axis\n"
        << "Expected: Same hash (rotation symmetry)\n";

    using namespace internal;
    detail::hasher<cylinder_paired_model_matrix> hasher;

    auto mat1 = std::make_unique<paired_model_matrix>();
    mat1->world_to_local = Eigen::Affine3d::Identity();

    auto mat2 = std::make_unique<paired_model_matrix>();
    Eigen::Affine3d t = Eigen::Affine3d::Identity();
    t.linear() = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ()).toRotationMatrix();
    mat2->world_to_local = t;

    cylinder_paired_model_matrix block1, block2;
    block1.data = mat1.get();
    block2.data = mat2.get();

    size_t h1 = hasher(block1);
    size_t h2 = hasher(block2);

    print_hash(h1, "Cylinder 1 (0 degree)");
    print_hash(h2, "Cylinder 2 (90 degree)");
    assert_equal_hashes(h1, h2, "Rotation around axis should not change hash");
    print_separator();
}

void test_cylinder_hasher_different_axis() {
    print_test_title("Test 3: Different Cylinder Axis");
    std::cout 
        << "Purpose: Verify hash changes when cylinder axis changes\n"
        << "Cylinder 1: Axis = Z\n"
        << "Cylinder 2: Axis = X\n"
        << "Expected: Different hashes\n";

    using namespace internal;
    detail::hasher<cylinder_paired_model_matrix> hasher;

    auto mat1 = std::make_unique<paired_model_matrix>();
    mat1->world_to_local = Eigen::Affine3d::Identity();

    auto mat2 = std::make_unique<paired_model_matrix>();
    Eigen::Affine3d t = Eigen::Affine3d::Identity();
    t.linear() = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitY()).toRotationMatrix(); // Z -> X
    mat2->world_to_local = t;

    cylinder_paired_model_matrix block1, block2;
    block1.data = mat1.get();
    block2.data = mat2.get();

    size_t h1 = hasher(block1);
    size_t h2 = hasher(block2);

    print_hash(h1, "Axis = Z");
    print_hash(h2, "Axis = X");
    if (h1 != h2) {
        std::cout << "   \033[32mPASSED\033[0m: Different axes produce different hashes" << std::endl;
    } else {
        std::cout << "   \033[31mFAILED\033[0m: Different axes should have different hashes" << std::endl;
        assert(false);
    }
    print_separator();
}

void test_cylinder_hasher_different_radius() {
    print_test_title("Test 4: Different Radius");
    std::cout 
        << "Purpose: Verify hash changes with radius\n"
        << "Cylinder 1: Radius = 1.0\n"
        << "Cylinder 2: Radius = 3.0\n"
        << "Expected: Different hashes\n";

    using namespace internal;
    detail::hasher<cylinder_paired_model_matrix> hasher;

    auto mat1 = std::make_unique<paired_model_matrix>();
    mat1->world_to_local = Eigen::Affine3d::Identity();

    auto mat2 = std::make_unique<paired_model_matrix>();
    Eigen::Affine3d t = Eigen::Affine3d::Identity();
    t.linear() = Eigen::DiagonalMatrix<double,3>(3.0, 3.0, 1.0) * t.linear(); // scale xy by 3
    mat2->world_to_local = t;

    cylinder_paired_model_matrix block1, block2;
    block1.data = mat1.get();
    block2.data = mat2.get();

    size_t h1 = hasher(block1);
    size_t h2 = hasher(block2);

    print_hash(h1, "Radius = 1.0");
    print_hash(h2, "Radius = 3.0");
    if (h1 != h2) {
        std::cout << "   \033[32mPASSED\033[0m: Different radii produce different hashes" << std::endl;
    } else {
        std::cout << "   \033[31mFAILED\033[0m: Different radii should have different hashes" << std::endl;
        assert(false);
    }
    print_separator();
}

void test_cylinder_hasher_translation_along_axis() {
    print_test_title("Test 5: Translation Along Cylinder Axis");
    std::cout 
        << "Purpose: Verify hash is invariant under translation along the cylinder's axis\n"
        << "   (i.e., sliding the cylinder along its own length)\n"
        << "Cylinder 1: Centered at origin\n"
        << "Cylinder 2: Translated by (0, 0, 100) — far along Z-axis\n"
        << "Expected: Same hash (axis-aligned translation should not affect equivalence)\n";

    using namespace internal;
    detail::hasher<cylinder_paired_model_matrix> hasher;

    // Cylinder 1: Identity (centered at origin)
    auto mat1 = std::make_unique<paired_model_matrix>();
    mat1->world_to_local = Eigen::Affine3d::Identity();

    // Cylinder 2: Same orientation and scale, but translated along Z-axis
    auto mat2 = std::make_unique<paired_model_matrix>();
    mat2->world_to_local.translation() = Eigen::Vector3d(0, 0, 100);

    cylinder_paired_model_matrix block1, block2;
    block1.data = mat1.get();
    block2.data = mat2.get();

    size_t h1 = hasher(block1);
    size_t h2 = hasher(block2);

    print_hash(h1, "Cylinder 1 (origin)");
    print_hash(h2, "Cylinder 2 (translated by (0,0,100))");

    assert_equal_hashes(h1, h2, "Translation along axis should not change hash");
    print_separator();
}

void test_cylinder_hasher_with_raw_matrix() {
    print_test_title("Test: Construct Cylinder from Raw Matrix Data");

    using namespace internal;
    detail::hasher<cylinder_paired_model_matrix> hasher;

    // 构造第一个圆柱
    auto mat1 = std::make_unique<paired_model_matrix>();
    Eigen::Matrix<double, 3, 4> m1;
    // 用你复制的 array 数据填充 m1
    m1 << 1.0, 0.0, 0.0, -0.5,
          0.0, 1.0, 0.0, 0.0,
          0.0, 0.0, 1.0, 0.0;
    mat1->world_to_local.matrix() = m1;

    // 构造第二个圆柱（可以用不同数据或同样数据做等价性测试）
    auto mat2 = std::make_unique<paired_model_matrix>();
    Eigen::Matrix<double, 3, 4> m2;
    m2 << 1.0, 0.0, 0.0, -0.5,
          0.0, 1.0, 0.0, 0.0,
          0.0, 0.0, 1.0, -1.0;
    mat2->world_to_local.matrix() = m2;

    cylinder_paired_model_matrix block1, block2;
    block1.data = mat1.get();
    block2.data = mat2.get();

    size_t h1 = hasher(block1);
    size_t h2 = hasher(block2);

    print_hash(h1, "Cylinder 1 (raw matrix)");
    print_hash(h2, "Cylinder 2 (raw matrix)");

    assert_equal_hashes(h1, h2, "Raw matrix cylinders should hash the same");
    print_separator();
}

int main() {
    std::cout << "\033[1;35m Starting Cylinder Hasher Tests\033[0m\n" 
              << std::string(60, '=') << "\n" << std::endl;

    test_cylinder_hasher_equivalence();
    test_cylinder_hasher_rotation_invariance();
    test_cylinder_hasher_different_axis();
    //test_cylinder_hasher_different_radius();
    test_cylinder_hasher_translation_along_axis();
    test_cylinder_hasher_with_raw_matrix();

    std::cout << "\033[1;32m All tests completed successfully!\033[0m" << std::endl;
    return 0;
}