#include <math/math_defs.hpp>
#include <subface/extrude/extrude_side_face.hpp>

namespace internal
{
double extrude_side_function_impl::eval_sdf(pointer_wrapper<subface> object, Eigen::Vector3d p)
{
    Eigen::Vector4d local_p = object->world_to_local * Eigen::Vector4d{p.x(), p.y(), p.z(), 1.0};

    extrude_axis* axis_         = reinterpret_cast<extrude_axis*>(object->additional_structure[0]);
    const auto    v             = axis_->get_closest_param(local_p.head<3>());
    auto [pattern_p, local_TBN] = axis_->to_pattern_local(v, local_p.head<3>());

    pattern*   pattern_ = reinterpret_cast<pattern*>(object->additional_structure[1]);
    const auto sdf      = pattern_->eval_sdf(pattern_p);

    return sdf;
}

Eigen::Vector3d extrude_side_function_impl::eval_sdf_grad(pointer_wrapper<subface> object, Eigen::Vector3d p)
{
    Eigen::Vector4d local_p = object->world_to_local * Eigen::Vector4d{p.x(), p.y(), p.z(), 1.0};
    Eigen::Vector2d normal  = local_p.head<2>().normalized();
    return object->trans_world_to_local_linear() * Eigen::Vector3d{normal.x(), normal.y(), 0.0};
}

Eigen::Vector2d extrude_side_function_impl::map_point_to_param(pointer_wrapper<subface> object, Eigen::Vector3d p)
{
    Eigen::Vector4d local_p = object->world_to_local * Eigen::Vector4d{p.x(), p.y(), p.z(), 1.0};
    return Eigen::Vector2d(corrected_atan2(local_p.y(), local_p.x()), local_p.z());
}

std::tuple<Eigen::Vector4d, double, double> extrude_side_function_impl::map_param_to_point_with_weight(
    pointer_wrapper<subface> object,
    Eigen::Vector2d          p)
{
    const auto cos_u = std::cos(p.x());
    const auto sin_u = std::sin(p.x());

    Eigen::Vector4d world_x = object->local_to_world * Eigen::Vector4d(cos_u, sin_u, p.y(), 1.0);
    Eigen::Vector4d world_n = (object->local_to_world * Eigen::Vector4d(cos_u, sin_u, .0, .0)).normalized();
    auto            x_dot_n = world_x.dot(world_n);

    const auto local_to_param_jacobi = 1.; // sin_u^2 + cos_u^2 = 1
    const auto volume_jacobi         = object->local_to_world.linear().determinant();
    const auto surface_jacobi =
        volume_jacobi * (object->trans_world_to_local_linear() * Eigen::Vector3d(cos_u, sin_u, .0)).norm();

    return {world_x, surface_jacobi * local_to_param_jacobi, volume_jacobi * local_to_param_jacobi * x_dot_n};
}

internal::constraint_curve_intermediate extrude_side_function_impl::eval_du_constraint(pointer_wrapper<subface> object,
                                                                                       Eigen::Vector2d          p)
{
    internal::constraint_curve_intermediate res{};

    const auto cos_u = std::cos(p.x());
    const auto sin_u = std::sin(p.x());

    res.f      = object->local_to_world * Eigen::Vector4d(cos_u, sin_u, p.y(), 1.0);
    res.grad_f = object->local_to_world * Eigen::Vector4d(-sin_u, cos_u, 0, 0.0);

    return res;
}

internal::constraint_curve_intermediate extrude_side_function_impl::eval_dv_constraint(pointer_wrapper<subface> object,
                                                                                       Eigen::Vector2d          p)
{
    internal::constraint_curve_intermediate res{};

    const auto cos_u = std::cos(p.x());
    const auto sin_u = std::sin(p.x());

    res.f      = object->local_to_world * Eigen::Vector4d(cos_u, sin_u, p.y(), 1.0);
    res.grad_f = object->local_to_world * Eigen::Vector4d(0, 0, 1, 0.0);

    return res;
}

internal::equation_intermediate_t extrude_side_function_impl::eval_intersection(
    pointer_wrapper<subface>                       object,
    const internal::constraint_curve_intermediate& res_on_other)
{
    internal::implicit_equation_intermediate res{};

    Eigen::Vector3d local_f = (object->world_to_local * res_on_other.f).head<3>();
    res.f                   = local_f.head<2>().squaredNorm() - 1;
    res.df                  = 2 * local_f.head<2>().dot((object->world_to_local * res_on_other.grad_f).head<2>());

    return res;
}

std::array<internal::periodical_state_t, 2> extrude_side_function_impl::decide_periodical_state(pointer_wrapper<subface> object,
                                                                                                Eigen::Vector4d local_p)
{
    return {common_decide_periodical_state(local_p), periodical_state_t::invalid};
}
} // namespace internal