use crate::constants::TILE_SIZE;
use crate::tile::TileMap;
use crate::{
    constants::*,
    entities::shared_components::Ambulatory,
    tilemap::{ChunkMap, CHUNK_SIZE},
};
use bevy::{math::ivec3, prelude::*};
use bevy_rand::prelude::*;
use rand::Rng;
use std::{
    collections::{BinaryHeap, HashMap, HashSet},
    process::exit,
};

#[derive(Clone, Eq, PartialEq, Debug)]
struct PathNode {
    position: IVec3,
    f_score: i32,
    g_score: i32,
}

impl Ord for PathNode {
    fn cmp(&self, other: &Self) -> std::cmp::Ordering {
        other.f_score.cmp(&self.f_score)
    }
}

impl PartialOrd for PathNode {
    fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
        Some(self.cmp(other))
    }
}

pub struct PathfindingPlugin;

impl Plugin for PathfindingPlugin {
    fn build(&self, app: &mut App) {
        app.add_systems(FixedUpdate, (update_wandering_targets, movement).chain());
    }
}

pub fn update_wandering_targets(
    mut query: Query<(&mut Ambulatory, &Transform)>, // add a 'with' here when behaviours are implemented
    tilemap: Res<TileMap>,
    chunk_map: Res<ChunkMap>,
    mut rng_q: Query<&mut Entropy<WyRand>, With<Global>>,
) {
    if let Ok(mut rng) = rng_q.single_mut() {
        for (mut ambulatory, _) in query.iter_mut() {
            if ambulatory.target.is_none()
                || (ambulatory.current_path.is_some()
                    && ambulatory.path_index >= ambulatory.current_path.as_ref().unwrap().len())
            {
                // Find a random loaded chunk
                let loaded_chunks: Vec<&IVec2> = chunk_map.loaded_chunks.keys().collect();
                if !loaded_chunks.is_empty() {
                    let random_index = rng.random_range(0..loaded_chunks.len());
                    if let Some(&chunk_pos) = loaded_chunks.get(random_index) {
                        // Generate random position within chunk
                        let chunk_x = chunk_pos.x * CHUNK_SIZE;
                        let chunk_y = chunk_pos.y * CHUNK_SIZE;

                        let target_x = chunk_x + rng.random_range(0..CHUNK_SIZE);
                        let target_y = chunk_y + rng.random_range(0..CHUNK_SIZE);

                        // Get height at position
                        let surface_height = 0;

                        // Find a valid z-level near the surface
                        for z in (surface_height - 3)..=(surface_height + 4) {
                            let mut target_pos = IVec3::new(target_x, target_y, z) * ITILE_SIZE;
                            if let Some(_) = tilemap.floor_tiles.get(&target_pos) {
                                target_pos.z += ITILE_SIZE;
                                if let Some(_base_texture) = tilemap.floor_tiles.get(&target_pos) {
                                    if is_standable_tile(&tilemap, target_pos) {
                                        ambulatory.target = Some(Vec3::new(
                                            target_pos.x as f32,
                                            target_pos.y as f32,
                                            target_pos.z as f32 + 1.0,
                                        ));
                                        ambulatory.current_path = None;
                                        ambulatory.path_index = 0;
                                        break;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    }
}

pub fn movement(mut query: Query<(&mut Ambulatory, &mut Transform)>, tilemap: Res<TileMap>) {
    query
        .par_iter_mut()
        .for_each(|(mut ambulatory, mut transform)| {
            let current_pos = transform.translation;
            // Apply gravity if in air
            if !is_standable_tile(&tilemap, current_pos.as_ivec3()) {
                transform.translation.z -= TILE_SIZE;
                return;
            }

            if let Some(target) = ambulatory.target {
                // Calculate path if needed
                if ambulatory.current_path.is_none() {
                    ambulatory.current_path = Some(calculate_path(
                        &tilemap,
                        transform.translation.as_ivec3(),
                        target.as_ivec3() - ivec3(0, 0, 1),
                    ));
                    ambulatory.path_index = 0;
                }
                if ambulatory.walk_speed > 0. {
                    if ambulatory.step_recovery <= ambulatory.walk_speed as u32 {
                        ambulatory.step_recovery += 1;
                        return;
                    } else {
                        ambulatory.step_recovery = 0;
                    }
                }

                // Follow the current path
                if let Some(path) = &ambulatory.current_path {
                    if ambulatory.path_index < path.len() {
                        let next_point = path[ambulatory.path_index];

                        let direction = (next_point - transform.translation).normalize();
                        transform.translation = next_point;

                        // Update sprite direction (only for x movement)
                        if direction.x > 0.0 {
                            transform.scale.x = PIXEL_RATIO;
                        } else if direction.x < 0.0 {
                            transform.scale.x = -PIXEL_RATIO;
                        }

                        // Check if we've reached the next point
                        if transform.translation.distance(next_point) < TILE_SIZE {
                            ambulatory.path_index += 1;
                        }
                    } else {
                        ambulatory.current_path = None;
                        ambulatory.target = None;
                    }
                }
            }
        });
}

fn is_standable_tile(tilemap: &TileMap, pos: IVec3) -> bool {
    let mut can_i_stand_in_tile: bool = false;
    let mut can_i_stand_on_tile_bellow: bool = false;
    let mut can_i_stand_in_fixture: bool = false;
    let mut can_i_stand_on_fixture_bellow: bool = false;

    // Check if current position has a blocking floor tile
    if let Some(current_floor_tile) = tilemap.floor_tiles.get(&pos) {
        can_i_stand_in_tile = current_floor_tile.1;
    }
    // Check if current position has a solid fixture tile (e.g., log)
    if let Some(current_fixture_tile) = tilemap.fixture_tiles.get(&pos) {
        can_i_stand_in_fixture = current_fixture_tile.1;
    }

    // Check if there's solid ground below (fixture or floor)
    let pos_below = pos - IVec3::new(0, 0, ITILE_SIZE);

    if let Some(below_floor_tile) = tilemap.floor_tiles.get(&pos_below) {
        can_i_stand_on_tile_bellow = below_floor_tile.2;
    }

    if let Some(below_fixture_tile) = tilemap.fixture_tiles.get(&pos_below) {
        can_i_stand_on_fixture_bellow = below_fixture_tile.2;
    }
    return (can_i_stand_in_tile || can_i_stand_in_fixture)
        && (can_i_stand_on_tile_bellow || can_i_stand_on_fixture_bellow);
}

fn calculate_path(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec<Vec3> {
    if !is_standable_tile(tilemap, start) {
        println!("Start pos invalid: {}", start);
        println!("Bugger (1)");
        exit(0);
    }
    if !is_standable_tile(tilemap, goal) {
        println!("Goal pos invalid: {}", goal);
        println!("Bugger (2)");
        exit(0);
    }

    let mut open_set = BinaryHeap::new();
    let mut came_from = HashMap::new();
    let mut g_scores = HashMap::new();
    let mut closed_set = HashSet::new();
    let mut in_open_set = HashSet::new();

    let start_node = PathNode {
        position: start,
        f_score: octile_distance_3d(start, goal),
        g_score: 0,
    };

    open_set.push(start_node);
    in_open_set.insert(start);
    g_scores.insert(start, 0);

    let allowed_moves = vec![
        // Orthogonal moves
        IVec3::new(-ITILE_SIZE, 0, 0),
        IVec3::new(ITILE_SIZE, 0, 0),
        IVec3::new(0, -ITILE_SIZE, 0),
        IVec3::new(0, ITILE_SIZE, 0),
        // Diagonal moves
        IVec3::new(-ITILE_SIZE, -ITILE_SIZE, 0),
        IVec3::new(-ITILE_SIZE, ITILE_SIZE, 0),
        IVec3::new(ITILE_SIZE, -ITILE_SIZE, 0),
        IVec3::new(ITILE_SIZE, ITILE_SIZE, 0),
        // Diagonal with vertical moves (left/negative preference)
        IVec3::new(-ITILE_SIZE, 0, ITILE_SIZE),
        IVec3::new(-ITILE_SIZE, 0, -ITILE_SIZE),
        IVec3::new(ITILE_SIZE, 0, ITILE_SIZE),
        IVec3::new(ITILE_SIZE, 0, -ITILE_SIZE),
        IVec3::new(0, -ITILE_SIZE, ITILE_SIZE),
        IVec3::new(0, -ITILE_SIZE, -ITILE_SIZE),
        IVec3::new(0, ITILE_SIZE, ITILE_SIZE),
        IVec3::new(0, ITILE_SIZE, -ITILE_SIZE),
        // Full 3D diagonal moves
        IVec3::new(-ITILE_SIZE, -ITILE_SIZE, ITILE_SIZE),
        IVec3::new(-ITILE_SIZE, -ITILE_SIZE, -ITILE_SIZE),
        IVec3::new(-ITILE_SIZE, ITILE_SIZE, ITILE_SIZE),
        IVec3::new(-ITILE_SIZE, ITILE_SIZE, -ITILE_SIZE),
        IVec3::new(ITILE_SIZE, -ITILE_SIZE, ITILE_SIZE),
        IVec3::new(ITILE_SIZE, -ITILE_SIZE, -ITILE_SIZE),
        IVec3::new(ITILE_SIZE, ITILE_SIZE, ITILE_SIZE),
        IVec3::new(ITILE_SIZE, ITILE_SIZE, -ITILE_SIZE),
    ];

    while let Some(current_node) = open_set.pop() {
        let current = current_node.position;

        in_open_set.remove(&current);

        if current == goal {
            // println!("path found");
            return reconstruct_path(came_from, current);
        }

        closed_set.insert(current);

        for &move_dir in &allowed_moves {
            let neighbor_pos = current + move_dir;

            if !is_standable_tile(tilemap, neighbor_pos) || closed_set.contains(&neighbor_pos) {
                continue;
            }

            // TODO: Add terrain-based cost modifiers
            // movement_cost = apply_terrain_modifier(movement_cost, neighbor_pos, tilemap);
            // Examples:
            // - Mud/sand: +50% cost
            // - Ice: +100% cost
            // - Designated high-traffic areas: -25% cost
            // - Designated restricted areas: +500% cost
            // - Etc

            let movement_cost = match (
                move_dir.x.abs() / ITILE_SIZE,
                move_dir.y.abs() / ITILE_SIZE,
                move_dir.z.abs() / ITILE_SIZE,
            ) {
                // 2D Movement (Dwarf Fortress style)
                (1, 0, 0) | (0, 1, 0) => 10, // Orthogonal movement
                (1, 1, 0) => 14,             // Diagonal movement (~√2 × 10)

                // Vertical Movement (Raw climbing - very expensive)
                // (0, 0, 1) => 50, // Pure vertical climb/fall

                // 3D Movement (Climbing diagonally - even more expensive)
                (1, 0, 1) | (0, 1, 1) => 52, // Orthogonal + vertical climb
                (1, 1, 1) => 56,             // Diagonal + vertical climb

                // TODO: Implement stairs and ramps for efficient vertical movement
                // Stairs would reduce vertical costs significantly:
                // (0, 0, 1) => 20 if has_stairs(current, neighbor_pos),     // Stairs: 2× horizontal cost
                // (1, 0, 1) | (0, 1, 1) => 24 if has_stairs(current, neighbor_pos), // Stairs + horizontal
                // (1, 1, 1) => 28 if has_stairs(current, neighbor_pos),     // Stairs + diagonal

                // TODO: Implement ramps for even smoother vertical movement
                // Ramps would be cheaper than stairs:
                // (0, 0, 1) => 15 if has_ramp(current, neighbor_pos),       // Ramps: 1.5× horizontal cost
                // (1, 0, 1) | (0, 1, 1) => 18 if has_ramp(current, neighbor_pos), // Ramps + horizontal
                // (1, 1, 1) => 21 if has_ramp(current, neighbor_pos),       // Ramps + diagonal
                _ => continue,
            };

            let new_g = g_scores.get(&current).unwrap_or(&i32::MAX) + movement_cost;

            if new_g < *g_scores.get(&neighbor_pos).unwrap_or(&i32::MAX) {
                came_from.insert(neighbor_pos, current);
                g_scores.insert(neighbor_pos, new_g);
                let h = octile_distance_3d(neighbor_pos, goal);
                let f = new_g + h;

                // Only add to open set if not already there
                if !in_open_set.contains(&neighbor_pos) {
                    let neighbor_node = PathNode {
                        position: neighbor_pos,
                        f_score: f,
                        g_score: new_g,
                    };
                    open_set.push(neighbor_node);
                    in_open_set.insert(neighbor_pos);
                } else {
                    let neighbor_node = PathNode {
                        position: neighbor_pos,
                        f_score: f,
                        g_score: new_g,
                    };
                    open_set.push(neighbor_node);
                }
            }
        }
    }

    Vec::new()
}

fn octile_distance_3d(a: IVec3, b: IVec3) -> i32 {
    let dx = (a.x - b.x).abs();
    let dy = (a.y - b.y).abs();
    let dz = (a.z - b.z).abs();

    // Dwarf Fortress style costs
    let cost_orthogonal = 10; // Horizontal orthogonal
    let cost_diagonal = 14; // Horizontal diagonal (~√2 × 10)
    let cost_climb = 50; // Raw vertical movement (climbing)

    let mut diffs = [dx, dy, dz];
    diffs.sort_unstable();
    let dmin = diffs[0];
    let dmax = diffs[2];

    if dz == 0 {
        // Pure 2D movement
        let diagonal_moves = dmin / ITILE_SIZE;
        let orthogonal_moves = (dmax - dmin) / ITILE_SIZE;
        cost_diagonal * diagonal_moves + cost_orthogonal * orthogonal_moves
    } else {
        // Movement involves Z - assume raw climbing for now
        // TODO: Modify this when stairs/ramps are implemented
        let z_moves = dz / ITILE_SIZE;
        let xy_distance = ((dx * dx + dy * dy) as f32).sqrt() as i32;
        let remaining_2d_diagonal = (xy_distance.min(dz)) / ITILE_SIZE;
        let remaining_2d_orthogonal =
            (xy_distance - remaining_2d_diagonal * ITILE_SIZE) / ITILE_SIZE;

        // Raw climbing cost + remaining 2D movement
        cost_climb * z_moves
            + cost_diagonal * remaining_2d_diagonal
            + cost_orthogonal * remaining_2d_orthogonal
    }
}

fn reconstruct_path(came_from: HashMap<IVec3, IVec3>, mut current: IVec3) -> Vec<Vec3> {
    let mut path = vec![Vec3::new(
        current.x as f32,
        current.y as f32,
        current.z as f32,
    )];

    while let Some(&previous) = came_from.get(&current) {
        path.push(Vec3::new(
            previous.x as f32,
            previous.y as f32,
            previous.z as f32,
        ));
        current = previous;
    }

    path.reverse();
    path
}