// SPDX-FileCopyrightText: 2025 Topola contributors
// SPDX-FileCopyrightText: 2024 Spade contributors
//
// SPDX-License-Identifier: MIT
//
//! Topological navmesh generation from Delaunay triangulation
// idea: see issue topola/topola#132

use alloc::collections::{btree_map::Entry, BTreeMap, BTreeSet};
use alloc::{boxed::Box, sync::Arc, vec::Vec};
use core::ops;
use num_traits::{Float, FloatConst};
use spade::{
    handles::{DirectedEdgeHandle, VoronoiVertex as VoroV},
    HasPosition, Point2, Triangulation,
};

use crate::{
    navmesh::{EdgeIndex, EdgePaths, NavmeshSer},
    DualIndex, Edge, NavmeshBase, NavmeshIndex, Node,
};

#[derive(Clone, Debug, PartialEq)]
pub struct TrianVertex<PNI, T: spade::SpadeNum> {
    pub idx: PNI,
    pub pos: Point2<T>,
}

impl<PNI, T: spade::SpadeNum> HasPosition for TrianVertex<PNI, T> {
    type Scalar = T;

    #[inline]
    fn position(&self) -> spade::Point2<T> {
        self.pos
    }
}

#[derive(Clone, Copy, Debug)]
struct EdgeMeta<PNI, Scalar> {
    //hypot: Scalar,
    /// `atan2`, but into the range `[0, 2π)`.
    angle: Scalar,

    data: Edge<PNI>,
}

impl<PNI, Scalar: Float + FloatConst + ops::AddAssign + ops::SubAssign> EdgeMeta<PNI, Scalar> {
    /// rotate the angle by 180°
    fn flip(&mut self) {
        self.data.flip();
        self.angle += Scalar::PI();
        if self.angle >= Scalar::TAU() {
            self.angle -= Scalar::TAU();
        }
    }
}

impl<B: NavmeshBase> NavmeshSer<B>
where
    B::Scalar: spade::SpadeNum
        + Float
        + FloatConst
        + num_traits::float::TotalOrder
        + ops::AddAssign
        + ops::SubAssign,
{
    pub fn from_triangulation<T>(triangulation: &T) -> Self
    where
        T: Triangulation<Vertex = TrianVertex<B::PrimalNodeIndex, B::Scalar>>,
    {
        let mut nodes =
            BTreeMap::<_, (Point2<B::Scalar>, BTreeSet<_>, Option<Point2<B::Scalar>>)>::new();
        // note that all the directions are in the range [0, 2π).
        let mut edges = BTreeMap::<
            EdgeIndex<NavmeshIndex<B::PrimalNodeIndex>>,
            EdgeMeta<B::PrimalNodeIndex, B::Scalar>,
        >::new();

        // insert edge for each Voronoi edge
        for edge in triangulation.undirected_voronoi_edges() {
            let edge = edge.as_directed();
            let (a_vert, b_vert) = (edge.from(), edge.to());
            let (a_idx, a) = insert_dual_node_position::<B, _, _, _>(&mut nodes, &a_vert, None);
            let (b_idx, b) =
                insert_dual_node_position::<B, _, _, _>(&mut nodes, &b_vert, Some(a_idx.clone()));
            nodes.get_mut(&a_idx).unwrap().1.insert(b_idx.clone());
            // https://docs.rs/spade/2.12.1/src/spade/delaunay_core/handles/public_handles.rs.html#305
            let delaunay = edge.as_delaunay_edge();
            insert_edge(
                &mut edges,
                a_idx,
                &a,
                b_idx,
                &b,
                Edge {
                    lhs: Some(delaunay.from().data().idx.clone()),
                    rhs: Some(delaunay.to().data().idx.clone()),
                },
            );
        }

        // insert edge for each {primal node} * {neighbors of that node}
        for node in triangulation.vertices() {
            // iterate over neighbors to generate sectors information
            let idx = NavmeshIndex::Primal(node.data().idx.clone());
            let a = node.data().pos;
            let mut primal_neighs = BTreeSet::new();

            for edge in node.as_voronoi_face().adjacent_edges() {
                // to convert dual edges around a node into dual nodes around a node,
                // we use the dual nodes that the edges point to.
                let dual_node = edge.to();
                let (dual_idx, b) = insert_dual_node_position::<B, _, _, _>(
                    &mut nodes,
                    &dual_node,
                    Some(idx.clone()),
                );
                primal_neighs.insert(dual_idx.clone());
                insert_edge(
                    &mut edges,
                    idx.clone(),
                    &a,
                    dual_idx,
                    &b,
                    Edge {
                        lhs: None,
                        rhs: None,
                    },
                );
            }
            assert!(nodes.insert(idx, (a, primal_neighs, None)).is_none());
        }

        // insert hull edges between `DualOuter` vertices
        {
            let convex_hull = triangulation.convex_hull().collect::<Vec<_>>();
            // if the convex hull only consists of two entries, we only have two primals, and the
            // convex hull generated by the code below would be invalid.
            // but given we want to find shortest paths, and for two entries that is unique,
            // we can just skip the generation in that case
            if convex_hull.len() > 2 {
                for cvhedges in convex_hull.windows(2) {
                    let [edge1, edge2] = cvhedges else {
                        continue;
                    };
                    insert_convex_hull_edge::<B, _, _, _>(&mut nodes, &mut edges, edge1, edge2);
                }
                insert_convex_hull_edge::<B, _, _, _>(
                    &mut nodes,
                    &mut edges,
                    &convex_hull[convex_hull.len() - 1],
                    &convex_hull[0],
                );
            }
        }

        let nodes = finalize_nodes::<B>(nodes, &edges);
        let empty_edge: EdgePaths<B::EtchedPath, B::GapComment> =
            Arc::from(Vec::new().into_boxed_slice());
        Self {
            nodes: Arc::new(nodes),
            edges: edges
                .into_iter()
                .map(|(k, emeta)| (k, (emeta.data, empty_edge.clone())))
                .collect(),
        }
    }
}

fn voronoi_vertex_get_index<V: HasPosition, DE, UE, F>(
    vertex: &spade::handles::VoronoiVertex<'_, V, DE, UE, F>,
) -> DualIndex {
    match vertex {
        VoroV::Inner(face) => DualIndex::Inner(face.index()),
        VoroV::Outer(out) => DualIndex::Outer(out.index()),
    }
}

// TODO: bound to root_bbox + some padding
fn voronoi_vertex_get_position<V, DE, UE, F>(
    vertex: &spade::handles::VoronoiVertex<'_, V, DE, UE, F>,
) -> (
    Point2<<V as HasPosition>::Scalar>,
    Option<Point2<<V as HasPosition>::Scalar>>,
)
where
    V: HasPosition,
    <V as HasPosition>::Scalar: num_traits::float::Float,
{
    match vertex {
        VoroV::Inner(face) => (face.circumcenter(), None),
        VoroV::Outer(halfspace) => {
            let delauney = halfspace.as_delaunay_edge();
            let from = delauney.from().position();
            let to = delauney.to().position();
            let orth = Point2 {
                x: -(to.y - from.y),
                y: to.x - from.x,
            };

            (
                Point2 {
                    x: (from.x + to.x) / ((2.0).into()) + orth.x,
                    y: (from.y + to.y) / ((2.0).into()) + orth.y,
                },
                Some(orth),
            )
        }
    }
}

fn insert_edge<PNI, Scalar>(
    edges: &mut BTreeMap<EdgeIndex<NavmeshIndex<PNI>>, EdgeMeta<PNI, Scalar>>,
    a_idx: NavmeshIndex<PNI>,
    a: &Point2<Scalar>,
    b_idx: NavmeshIndex<PNI>,
    b: &Point2<Scalar>,
    data: Edge<PNI>,
) where
    PNI: Ord,
    Scalar: Float + FloatConst + ops::AddAssign + ops::SubAssign,
{
    let direction = Point2 {
        x: b.x - a.x,
        y: b.y - a.y,
    };

    let angle = direction.y.atan2(direction.x);
    let mut edgemeta = EdgeMeta {
        // hypot: Scalar::hypot(direction.x, direction.y),
        angle: if angle.is_sign_negative() {
            angle + Scalar::TAU()
        } else {
            angle
        },
        data,
    };

    if a_idx > b_idx {
        edgemeta.flip();
    }

    edges.insert(EdgeIndex::from((a_idx, b_idx)), edgemeta);
}

fn insert_dual_node_position<B: NavmeshBase, DE, UE, F>(
    nodes: &mut BTreeMap<
        NavmeshIndex<B::PrimalNodeIndex>,
        (
            Point2<B::Scalar>,
            BTreeSet<NavmeshIndex<B::PrimalNodeIndex>>,
            Option<Point2<B::Scalar>>,
        ),
    >,
    vertex: &spade::handles::VoronoiVertex<
        '_,
        TrianVertex<B::PrimalNodeIndex, B::Scalar>,
        DE,
        UE,
        F,
    >,
    new_neighbor: Option<NavmeshIndex<B::PrimalNodeIndex>>,
) -> (NavmeshIndex<B::PrimalNodeIndex>, Point2<B::Scalar>)
where
    B::Scalar: spade::SpadeNum + Float,
{
    let idx = NavmeshIndex::Dual(voronoi_vertex_get_index(vertex));
    let ret = match nodes.entry(idx.clone()) {
        Entry::Occupied(occ) => occ.into_mut(),
        Entry::Vacant(vac) => {
            let (x, x_odir) = voronoi_vertex_get_position(vertex);
            vac.insert((x, BTreeSet::new(), x_odir))
        }
    };
    if let Some(neigh) = new_neighbor {
        ret.1.insert(neigh);
    }
    (idx, ret.0)
}

fn insert_convex_hull_edge<B: NavmeshBase, DE, UE, F>(
    nodes: &mut BTreeMap<
        NavmeshIndex<B::PrimalNodeIndex>,
        (
            Point2<B::Scalar>,
            BTreeSet<NavmeshIndex<B::PrimalNodeIndex>>,
            Option<Point2<B::Scalar>>,
        ),
    >,
    edges: &mut BTreeMap<
        EdgeIndex<NavmeshIndex<B::PrimalNodeIndex>>,
        EdgeMeta<B::PrimalNodeIndex, B::Scalar>,
    >,
    edge1: &DirectedEdgeHandle<TrianVertex<B::PrimalNodeIndex, B::Scalar>, DE, UE, F>,
    edge2: &DirectedEdgeHandle<TrianVertex<B::PrimalNodeIndex, B::Scalar>, DE, UE, F>,
) where
    B::Scalar: spade::SpadeNum + Float + FloatConst + ops::AddAssign + ops::SubAssign,
{
    let (edge1to, edge2to) = (
        edge1.as_voronoi_edge().from(),
        edge2.as_voronoi_edge().from(),
    );
    assert!(matches!(edge1to, VoroV::Outer(_)));
    assert!(matches!(edge2to, VoroV::Outer(_)));

    let a_idx = NavmeshIndex::Dual(voronoi_vertex_get_index(&edge1to));
    let b_idx = NavmeshIndex::Dual(voronoi_vertex_get_index(&edge2to));

    let a_pos = {
        let a = nodes.get_mut(&a_idx).unwrap();
        a.1.insert(b_idx.clone());
        a.0
    };
    let b_pos = {
        let b = nodes.get_mut(&b_idx).unwrap();
        b.1.insert(a_idx.clone());
        b.0
    };

    // > The edges are returned in clockwise order as seen from any point in the triangulation.
    let rhs_prim = edge1.to().data().idx.clone();
    debug_assert!(edge2.from().data().idx == rhs_prim);

    insert_edge(
        edges,
        a_idx,
        &a_pos,
        b_idx,
        &b_pos,
        Edge {
            lhs: None,
            rhs: Some(rhs_prim),
        },
    );
}

/// extracts sector information
fn finalize_nodes<B: NavmeshBase>(
    nodes: BTreeMap<
        NavmeshIndex<B::PrimalNodeIndex>,
        (
            Point2<B::Scalar>,
            BTreeSet<NavmeshIndex<B::PrimalNodeIndex>>,
            Option<Point2<B::Scalar>>,
        ),
    >,
    edges: &BTreeMap<
        EdgeIndex<NavmeshIndex<B::PrimalNodeIndex>>,
        EdgeMeta<B::PrimalNodeIndex, B::Scalar>,
    >,
) -> BTreeMap<NavmeshIndex<B::PrimalNodeIndex>, Node<B::PrimalNodeIndex, B::Scalar>>
where
    B::Scalar: Float + FloatConst + num_traits::float::TotalOrder + ops::AddAssign + ops::SubAssign,
{
    nodes
        .into_iter()
        .map(|(idx, (pos, neighs, open_direction))| {
            let mut neighs: Box<[_]> = neighs.into_iter().collect();
            neighs.sort_by(|a, b| {
                let mut dir_a = edges[&EdgeIndex::from((idx.clone(), a.clone()))].clone();
                if idx > *a {
                    dir_a.flip();
                }
                let mut dir_b = edges[&EdgeIndex::from((idx.clone(), b.clone()))].clone();
                if idx > *b {
                    dir_b.flip();
                }
                <B::Scalar as num_traits::float::TotalOrder>::total_cmp(&dir_a.angle, &dir_b.angle)
            });
            (
                idx,
                Node {
                    neighs,
                    pos,
                    open_direction,
                },
            )
        })
        .collect()
}