#include "subface/simple/cylinder_face.hpp"

namespace internal
{
auto cylinder_face_t::fetch_sdf_evaluator() const -> std::function<double(Eigen::Vector3d)>
{
    auto functor = [](Eigen::Vector3d p, Eigen::Vector3d local_to_world_scale, const transform_block& _world_to_local) {
        Eigen::Vector4d local_p = _world_to_local * Eigen::Vector4d{p.x(), p.y(), p.z(), 1.0};
        const double    len     = local_p.head<2>().norm();

        if (len > std::numeric_limits<double>::epsilon()) {
            Eigen::Vector2d normal = local_p.head<2>() / len;
            return local_to_world_scale.head<2>().cwiseProduct(normal).norm() * (len * len - 1);
        } else {
            return -local_to_world_scale.minCoeff();
        }
    };

    return std::bind(functor, std::placeholders::_1, this->local_to_world_scale(), std::ref(this->raw_world_to_local()));
}

auto cylinder_face_t::fetch_sdf_grad_evaluator() const -> std::function<Eigen::Vector3d(Eigen::Vector3d)>
{
    auto functor = [](Eigen::Vector3d p, Eigen::Matrix3d trans_world_to_local_linear, const transform_block& _world_to_local) {
        Eigen::Vector4d local_p = _world_to_local * Eigen::Vector4d{p.x(), p.y(), p.z(), 1.0};
        Eigen::Vector2d normal  = local_p.head<2>().normalized();
        return trans_world_to_local_linear * Eigen::Vector3d{normal.x(), normal.y(), 0.0};
    };

    return std::bind(functor, std::placeholders::_1, this->trans_world_to_local_linear(), std::ref(this->raw_world_to_local()));
}

auto cylinder_face_t::fetch_point_by_param_evaluator() const -> std::function<Eigen::Vector4d(double, double)>
{
    auto functor = [](double u, double v, const transform_block& _local_to_world) {
        const auto cos_u = std::cos(u);
        const auto sin_u = std::sin(u);
        return _local_to_world * Eigen::Vector4d(cos_u, v, sin_u, 1.0);
    };

    return std::bind(functor, std::placeholders::_1, std::placeholders::_2, std::ref(this->raw_local_to_world()));
}

auto cylinder_face_t::fetch_param_mapping_evaluator() const -> std::function<Eigen::Vector2d(Eigen::Vector3d)>
{
    auto functor = [](Eigen::Vector3d p, const transform_block* _world_to_local) {
        Eigen::Vector4d local_p = *_world_to_local * Eigen::Vector4d{p.x(), p.y(), p.z(), 1.0};
        return Eigen::Vector2d(std::atan2(local_p.y(), local_p.x()), local_p.z());
    };

    return std::bind(functor, std::placeholders::_1, &this->raw_world_to_local());
}

auto cylinder_face_t::fetch_curve_constraint_evaluator(parameter_u_t constraint_var_type, double u) const
    -> std::function<constraint_curve_intermediate(double)>
{
    const auto cos_u = std::cos(u);
    const auto sin_u = std::sin(u);

    auto functor = [&](double v, const transform_block& _local_to_world) {
        constraint_curve_intermediate res{};
        res.f      = _local_to_world * Eigen::Vector4d(cos_u, v, sin_u, 1.0);
        res.grad_f = _local_to_world * Eigen::Vector4d(0, 1, 0, 0.0);

        return res;
    };

    return std::bind(functor, std::placeholders::_1, std::ref(this->raw_local_to_world()));
}

auto cylinder_face_t::fetch_curve_constraint_evaluator(parameter_v_t constraint_var_type, double v) const
    -> std::function<constraint_curve_intermediate(double)>
{
    auto functor = [&](double u, const transform_block& _local_to_world) {
        auto cos_u = std::cos(u);
        auto sin_u = std::sin(u);

        constraint_curve_intermediate res{};
        res.f      = _local_to_world * Eigen::Vector4d(cos_u, v, sin_u, 1.0);
        res.grad_f = _local_to_world * Eigen::Vector4d(-sin_u, 0.0, cos_u, 0.0);

        return res;
    };

    return std::bind(functor, std::placeholders::_1, std::ref(this->raw_local_to_world()));
}

auto cylinder_face_t::fetch_solver_evaluator() const -> std::function<equation_intermediate_t(constraint_curve_intermediate&&)>
{
    auto functor = [](constraint_curve_intermediate&& temp_res, const transform_block& _world_to_local) {
        implicit_equation_intermediate res{};
        Eigen::Vector3d                local_f = (_world_to_local * temp_res.f).head<3>();
        res.f                                  = local_f.head<2>().squaredNorm() - 1;
        res.df                                 = 2 * local_f.head<2>().dot((_world_to_local * temp_res.grad_f).head<2>());
        return res;
    };

    return std::bind(functor, std::placeholders::_1, std::ref(this->raw_world_to_local()));
}
} // namespace internal