kengaai-navigation 0.1.1

use glam::{Vec3, Vec2};
use serde::{Deserialize, Serialize};
use anyhow::Result;

#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
struct Vec3Ser {
    x: f32,
    y: f32,
    z: f32,
}

impl From<Vec3> for Vec3Ser {
    fn from(v: Vec3) -> Self {
        Self { x: v.x, y: v.y, z: v.z }
    }
}

impl From<Vec3Ser> for Vec3 {
    fn from(v: Vec3Ser) -> Self {
        Vec3::new(v.x, v.y, v.z)
    }
}

/// Треугольник навигационной сетки
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct NavTriangle {
    #[serde(with = "serde_vertices")]
    pub vertices: [Vec3; 3],
    pub neighbors: Vec<usize>, // Индексы соседних треугольников
    #[serde(with = "serde_vec3")]
    pub center: Vec3,
    pub area: f32,
}

mod serde_vec3 {
    use super::*;
    use serde::{Deserialize, Deserializer, Serialize, Serializer};

    pub fn serialize<S>(v: &Vec3, serializer: S) -> Result<S::Ok, S::Error>
    where
        S: Serializer,
    {
        Vec3Ser::from(*v).serialize(serializer)
    }

    pub fn deserialize<'de, D>(deserializer: D) -> Result<Vec3, D::Error>
    where
        D: Deserializer<'de>,
    {
        Vec3Ser::deserialize(deserializer).map(Vec3::from)
    }
}

mod serde_vertices {
    use super::*;
    use serde::{Deserialize, Deserializer, Serialize, Serializer};

    pub fn serialize<S>(v: &[Vec3; 3], serializer: S) -> Result<S::Ok, S::Error>
    where
        S: Serializer,
    {
        let ser: [Vec3Ser; 3] = [Vec3Ser::from(v[0]), Vec3Ser::from(v[1]), Vec3Ser::from(v[2])];
        ser.serialize(serializer)
    }

    pub fn deserialize<'de, D>(deserializer: D) -> Result<[Vec3; 3], D::Error>
    where
        D: Deserializer<'de>,
    {
        let ser: [Vec3Ser; 3] = Deserialize::deserialize(deserializer)?;
        Ok([Vec3::from(ser[0]), Vec3::from(ser[1]), Vec3::from(ser[2])])
    }
}

impl NavTriangle {
    pub fn new(vertices: [Vec3; 3]) -> Self {
        let center = (vertices[0] + vertices[1] + vertices[2]) / 3.0;
        let v1 = vertices[1] - vertices[0];
        let v2 = vertices[2] - vertices[0];
        let area = v1.cross(v2).length() / 2.0;
        
        Self {
            vertices,
            neighbors: Vec::new(),
            center,
            area,
        }
    }

    /// Проверяет, находится ли точка внутри треугольника (в 2D, по XZ плоскости)
    pub fn contains_point_2d(&self, point: Vec3) -> bool {
        let p = Vec2::new(point.x, point.z);
        let v0 = Vec2::new(self.vertices[0].x, self.vertices[0].z);
        let v1 = Vec2::new(self.vertices[1].x, self.vertices[1].z);
        let v2 = Vec2::new(self.vertices[2].x, self.vertices[2].z);
        
        // Используем cross product для проверки ориентации
        let sign1 = (p - v0).dot(Vec2::new(-(v1 - v0).y, (v1 - v0).x));
        let sign2 = (p - v1).dot(Vec2::new(-(v2 - v1).y, (v2 - v1).x));
        let sign3 = (p - v2).dot(Vec2::new(-(v0 - v2).y, (v0 - v2).x));
        
        (sign1 >= 0.0 && sign2 >= 0.0 && sign3 >= 0.0) || (sign1 <= 0.0 && sign2 <= 0.0 && sign3 <= 0.0)
    }
}

/// Навигационная сетка
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct NavMesh {
    pub triangles: Vec<NavTriangle>,
    #[serde(with = "serde_vec3")]
    pub bounds_min: Vec3,
    #[serde(with = "serde_vec3")]
    pub bounds_max: Vec3,
}

impl NavMesh {
    pub fn new() -> Self {
        Self {
            triangles: Vec::new(),
            bounds_min: Vec3::splat(f32::MAX),
            bounds_max: Vec3::splat(f32::MIN),
        }
    }

    /// Добавляет треугольник в сетку
    pub fn add_triangle(&mut self, triangle: NavTriangle) {
        // Обновляем границы
        for vertex in &triangle.vertices {
            self.bounds_min = self.bounds_min.min(*vertex);
            self.bounds_max = self.bounds_max.max(*vertex);
        }
        self.triangles.push(triangle);
    }

    /// Находит треугольник, содержащий точку
    pub fn find_triangle(&self, point: Vec3) -> Option<usize> {
        for (i, triangle) in self.triangles.iter().enumerate() {
            if triangle.contains_point_2d(point) {
                return Some(i);
            }
        }
        None
    }

    /// Находит ближайший треугольник к точке
    pub fn find_nearest_triangle(&self, point: Vec3) -> Option<usize> {
        let mut nearest_index = None;
        let mut nearest_distance = f32::MAX;

        for (i, triangle) in self.triangles.iter().enumerate() {
            let distance = triangle.center.distance(point);
            if distance < nearest_distance {
                nearest_distance = distance;
                nearest_index = Some(i);
            }
        }

        nearest_index
    }
}

impl Default for NavMesh {
    fn default() -> Self {
        Self::new()
    }
}

/// Система поиска пути (упрощенная версия A*)
pub struct Pathfinder {
    nav_mesh: NavMesh,
}

impl Pathfinder {
    pub fn new(nav_mesh: NavMesh) -> Self {
        Self { nav_mesh }
    }

    /// Находит путь от начальной точки до конечной
    pub fn find_path(&self, start: Vec3, end: Vec3) -> Result<Vec<Vec3>> {
        let start_triangle = self.nav_mesh.find_triangle(start)
            .or_else(|| self.nav_mesh.find_nearest_triangle(start))
            .ok_or_else(|| anyhow::anyhow!("Start point not on nav mesh"))?;
        
        let end_triangle = self.nav_mesh.find_triangle(end)
            .or_else(|| self.nav_mesh.find_nearest_triangle(end))
            .ok_or_else(|| anyhow::anyhow!("End point not on nav mesh"))?;

        if start_triangle == end_triangle {
            return Ok(vec![start, end]);
        }

        // Упрощенный поиск пути через соседние треугольники
        let mut path = vec![start];
        let mut current = start_triangle;
        let mut visited = std::collections::HashSet::new();
        visited.insert(current);

        // Простой жадный алгоритм движения к цели
        while current != end_triangle {
            let current_triangle = &self.nav_mesh.triangles[current];
            let mut best_neighbor = None;
            let mut best_distance = f32::MAX;

            for &neighbor_idx in &current_triangle.neighbors {
                if visited.contains(&neighbor_idx) {
                    continue;
                }

                let neighbor = &self.nav_mesh.triangles[neighbor_idx];
                let distance = neighbor.center.distance(end);
                if distance < best_distance {
                    best_distance = distance;
                    best_neighbor = Some(neighbor_idx);
                }
            }

            if let Some(next) = best_neighbor {
                path.push(self.nav_mesh.triangles[next].center);
                current = next;
                visited.insert(current);
            } else {
                // Не можем найти путь
                break;
            }
        }

        path.push(end);
        Ok(path)
    }
}