#include <algorithm/glue_algorithm.hpp>

#include "internal_primitive_desc.hpp"

namespace internal
{
void polyline::build_as_axis(const polyline_descriptor_t            &desc,
                             const raw_vector3d_t                   &profile_ref_normal,
                             Eigen::Ref<Eigen::Matrix<double, 3, 4>> axis_to_world,
                             aabb_t<>                               &aabb)
{
    Eigen::Vector3d origin, topper;
    vec3d_conversion(desc.axis_start, origin);
    vec3d_conversion(desc.axis_end, topper);
    this->radius      = desc.radius;
    this->height      = (topper - origin).norm();
    this->total_theta = this->height / desc.advance_per_round * TWO_PI;

    auto &matrix_handle = axis_to_world.matrix();
    vec3d_conversion(desc.start_direction, matrix_handle.col(0));
    matrix_handle.col(0).normalized();
    matrix_handle.col(2) = (topper - origin) / this->height;
    matrix_handle.col(3) = origin;
    matrix_handle.col(1) = matrix_handle.col(2).cross(matrix_handle.col(0));

    aabb = {
        Eigen::Vector3d{-this->radius, -this->radius, 0           },
        Eigen::Vector3d{this->radius,  this->radius,  this->height}
    };
}

helixline::helixline(helixline_descriptor_t &&desc, Eigen::Ref<Eigen::Matrix<double, 3, 4>> &axis_to_world, aabb_t<> &aabb)
{
    Eigen::Vector3d origin, topper;
    vec3d_conversion(std::move(desc.axis_start), origin);
    vec3d_conversion(std::move(desc.axis_end), topper);
    this->radius      = std::move(desc.radius);
    this->height      = (topper - origin).norm();
    this->total_theta = this->height / std::move(desc.advance_per_round) * TWO_PI;

    auto &matrix_handle = axis_to_world.matrix();
    vec3d_conversion(std::move(desc.start_direction), matrix_handle.col(0));
    matrix_handle.col(0).normalized();
    matrix_handle.col(2) = (topper - origin) / this->height;
    matrix_handle.col(3) = origin;
    matrix_handle.col(1) = matrix_handle.col(2).cross(matrix_handle.col(0));

    aabb = {
        Eigen::Vector3d{-this->radius, -this->radius, 0           },
        Eigen::Vector3d{this->radius,  this->radius,  this->height}
    };
}

[[nodiscard]] Eigen::Vector3d helixline::calculate_tangent(double t) const
{
    assert(t >= 0 && t <= 1);

    return Eigen::Vector3d{-this->total_theta * this->radius * std::sin(t * this->total_theta),
                           this->total_theta * this->radius * std::cos(t * this->total_theta),
                           this->height}
        .normalized();
}

[[nodiscard]] Eigen::Vector3d helixline::calculate_normal(double t) const
{
    assert(t >= 0 && t <= 1);

    return {-std::cos(t * this->total_theta), -std::sin(t * this->total_theta), 0.};
}

[[nodiscard]] line_closest_param_t helixline::calculate_closest_param(const Eigen::Ref<const Eigen::Vector3d> &p) const
{
    const auto h_p             = p.z();
    const auto theta_p         = std::atan2(p.y(), p.x());
    const auto r_p             = p.topRows<2>().norm();
    const auto theta_intersect = this->total_theta * h_p / this->height;
    const auto k               = (theta_intersect - theta_p) * INV_TWO_PI;
    const auto rounded_k       = std::round(k);

    // early exit case: point p is on the helix
    if (std::abs(k - rounded_k) <= EPSILON) {
        const auto theta = theta_p + rounded_k * TWO_PI;
        if (theta >= 0 && theta <= this->total_theta)
            return {
                {p.x(), p.y(), p.z(), 1},
                theta / this->total_theta,
                true
            };
        else {
            // for points two sides away from the helix, this may cause slight numerical error
            // but it should be fine since this kind of points are rare
            const auto clipped_theta = std::clamp(theta, 0., this->total_theta);
            const auto t             = clipped_theta / this->total_theta;
            return {
                {this->radius * std::cos(t * this->total_theta),
                 this->radius * std::sin(t * this->total_theta),
                 t * this->height,
                 1},
                std::move(t),
                false
            };
        }
    }

    const auto times_p             = -theta_p * (1 + this->total_theta) * INV_PI;
    const auto times_line          = this->total_theta * this->total_theta * INV_PI;
    const auto rounded_times_start = std::ceil(times_p - 0.5);
    const auto rounded_times_end   = std::floor(times_line + times_p - 0.5);

    const auto line_theta_r   = this->total_theta * this->radius;
    const auto inv_line_theta = 1. / this->total_theta;
    const auto t_intersect    = std::sqrt((theta_intersect * theta_intersect * this->radius * this->radius + h_p * h_p)
                                       / (line_theta_r * line_theta_r + this->height * this->height));

    struct line_simple_distance_param_t {
        double t{};
        bool   need_refine{false};
        bool   is_peak_value{true};
    } peak_value_param{};

    // collect all peak values of f(t) in domain [0, 1], and then find the interval which:
    // 1. has one root as extreme minimum point
    // 2. the root has smallest distance to t_intersect
    {
        const auto                             alpha = this->total_theta * this->radius * r_p;
        std::vector<std::pair<double, double>> peak_values(rounded_times_end - rounded_times_start + 1 + 2);
        peak_values.front() = {.0, -line_theta_r * r_p * std::sin(theta_p) - h_p};
        peak_values.back()  = {1., line_theta_r * r_p * std::sin(this->total_theta - theta_p) + this->height - h_p};
        algorithm::transform(counting_iterator<size_t>(rounded_times_start),
                             counting_iterator<size_t>(rounded_times_end + 1),
                             peak_values.begin() + 1,
                             [&](size_t k_) -> std::pair<double, double> {
                                 const auto t = (theta_p + PI2 + k_ * PI) * inv_line_theta;
                                 return {std::move(t),
                                         alpha * std::sin(t * this->total_theta - theta_p) + t * this->height - h_p};
                             });
        peak_value_param = algorithm::transform_reduce(
            counting_iterator<size_t>{},
            counting_iterator<size_t>{peak_values.size() - 1},
            line_simple_distance_param_t{},
            [&](const line_simple_distance_param_t &lhs, const line_simple_distance_param_t &rhs) {
                if (std::abs(lhs.t - t_intersect) < std::abs(rhs.t - t_intersect))
                    return lhs;
                else
                    return rhs;
            },
            [&](size_t index) -> line_simple_distance_param_t {
                const auto &[t0, f0] = peak_values[index];
                const auto &[t1, f1] = peak_values[index + 1];
                bool increasing_flag = (f0 > 0 && f1 > 0);
                bool decreasing_flag = (f0 < 0 && f1 < 0);
                // i.e. has a root t=t0, or distance is always increasing as t increases
                if ((f0 >= -EPSILON && f0 <= EPSILON) || increasing_flag) return {t0, false, !increasing_flag};
                // i.e. has a root t=t1, or distance is always decreasing as t increases
                if ((f1 >= -EPSILON && f1 <= EPSILON) || decreasing_flag) return {t1, false, !decreasing_flag};
                // i.e. has a root t in (0, 1)
                // in this case, we use linear interpolation as a guess
                return {f0 / (f0 - f1), true, true};
            });
    }

    // if we need to refine the guess, just do it!!!
    if (peak_value_param.need_refine) {
        auto      &t = peak_value_param.t;
        double     delta_t{std::numeric_limits<double>::max()};
        const auto alpha       = this->total_theta * this->radius * r_p;
        const auto alpha_deriv = alpha * this->total_theta;
        while (std::abs(delta_t) >= EPSILON) {
            // origin equation: f(t)=theta_all*r_all*r_p*sin(t*theta_all-theta_p)+t*height_all-h_p=0
            // derivative: f'(t)=theta_all^2*r_all*r_p*cos(t*theta_all-theta_p)+height_all=0
            const auto f        = alpha * std::sin(t * this->total_theta - theta_p) + t * this->height - h_p;
            const auto f_deriv  = alpha_deriv * std::cos(t * this->total_theta - theta_p) + this->height;
            delta_t             = f / f_deriv;
            t                  -= delta_t;
        }
    }

    // get the final result
    return {
        {this->radius * std::cos(peak_value_param.t * this->total_theta),
         this->radius * std::sin(peak_value_param.t * this->total_theta),
         peak_value_param.t * this->height,
         1},
        peak_value_param.t,
        peak_value_param.is_peak_value
    };
}
} // namespace internal