#include "subface/simple/sphere_face.hpp"

namespace internal
{
auto sphere_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<3>().norm();

        if (len > std::numeric_limits<double>::epsilon()) {
            Eigen::Vector3d normal = local_p.head<3>() / len;
            return local_to_world_scale.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(), &this->raw_world_to_local());
}

auto sphere_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::Vector3d normal  = local_p.head<3>().normalized();
        return trans_world_to_local_linear * normal;
    };

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

auto sphere_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);
        const auto cos_v = std::cos(v);
        const auto sin_v = std::sin(v);
        return *_local_to_world * Eigen::Vector4d(cos_u * cos_v, sin_v, sin_u * cos_v, 1.0);
    };

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

auto sphere_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) {
        auto cos_v = std::cos(v);
        auto sin_v = std::sin(v);

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

        return res;
    };

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

auto sphere_face_t::fetch_curve_constraint_evaluator(parameter_v_t constraint_var_type, double v) const
    -> std::function<constraint_curve_intermediate(double)>
{
    const auto cos_v = std::cos(v);
    const auto sin_v = std::sin(v);

    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 * cos_v, sin_v, sin_u * cos_v, 1.0);
        res.grad_f = *_local_to_world * Eigen::Vector4d(-sin_u * cos_v, 0.0, cos_u * cos_v, 0.0);

        return res;
    };

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

auto sphere_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.squaredNorm() - 1;
        res.df                                 = 2 * local_f.dot((*_world_to_local * temp_res.grad_f).head<3>());
        return res;
    };

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