#pragma once

#include <base/subface.hpp>
#include <primitive_descriptor.h>

namespace internal
{
// local: cylinder face x^2+y^2-1=0
// u: planar angle from x-axis to z-axis
// v: depth/height from xz-plane to y-axis
    struct cylinder_face_t final : subface {
    const cylinder_descriptor_t* geometry_ptr = nullptr;

    // 内部访问：类型安全，无需转换
    const cylinder_descriptor_t* get_geometry_ptr() const
    {
        assert(geometry_ptr && "Geometry not initialized");
        return geometry_ptr;
    }
    };
} // namespace internal

namespace detail
{
template <>
struct hasher<internal::cylinder_face_t> {
    // Q' = M^{-T}QM^{-1}
    // Q = {
    // 1, 0, 0, 0
    // 0, 1, 0, 0
    // 0, 0, 0, 0
    // 0, 0, 0, -1
    // }
    size_t operator()(const internal::cylinder_face_t& subface) const
    {
        // build M^{-T}Q directly
        Eigen::Matrix4d mat = internal::empty_affine_matrix;
        mat.leftCols<2>()   = subface.world_to_local.matrix().topRows<2>().transpose();
        auto   res          = mat * subface.world_to_local;
        size_t h            = hash_funcs(res.matrix());
        detail::hash_combine(h, reinterpret_cast<size_t>(subface.geometry_ptr));
        return h;
    }

    // size_t operator()(const internal::cylinder_face_t& subface) const
    // {
    //     auto R = subface.world_to_local.linear().topLeftCorner<2, 3>(); // 2x3
    //     auto b = subface.local_to_world.translation();                  // 3x1

    //     Eigen::Matrix<double, 3, 4> hash_mat = Eigen::Matrix<double, 3, 4>::Zero();
    //     hash_mat.topLeftCorner<3, 3>()       = R.transpose() * R;
    //     hash_mat.topRightCorner<2, 1>()      = R * b;

    //     return hash_funcs(hash_mat);
    // }
};

template <>
struct eq_compare<internal::cylinder_face_t> {
    bool operator()(const internal::cylinder_face_t& lhs, const internal::cylinder_face_t& rhs) const
    {
        constexpr double eps = 1e-12;
        if (lhs.geometry_ptr != rhs.geometry_ptr) {
            return false;
        }
        if (!lhs.world_to_local.isApprox(rhs.world_to_local, 1e-12)) {
            return false;
        }
        return true;
    }
};
} // namespace detail