dorf/src/entities/tasks/job_pathfinding.rs

use bevy::prelude::*;
use smallvec::SmallVec;

use crate::config::GameConfig;
use crate::entities::behaviour::EntityType;
use crate::entities::shared_components::Ambulatory;
use crate::entities::shared_systems::pathfinding::calculate_traversal_distance;
use crate::entities::tasks::components::{ChopStep, HaulStep, Task, TaskQueue, TaskState};
use crate::entities::tasks::job_queue::{JobId, JobKind, JobQueue, JobState};
use crate::world::tiles::TileMap;

const MAX_JOBS_PER_TICK: usize = 5;

pub fn job_pathfinding_system(
    mut job_queue: ResMut<JobQueue>,
    config: Res<GameConfig>,
    tilemap: Res<TileMap>,
    mut dorf_query: Query<
        (
            Entity,
            &mut TaskQueue,
            &mut TaskState,
            &Transform,
            &mut Ambulatory,
        ),
        With<EntityType>,
    >,
) {
    let mut unclaimed: Vec<usize> = job_queue.iter_unclaimed().map(|(idx, _)| idx).collect();

    if unclaimed.is_empty() {
        return;
    }

    unclaimed.sort_by(|&a, &b| {
        let pri_a = job_queue.get_job_priority(a).unwrap_or(0);
        let pri_b = job_queue.get_job_priority(b).unwrap_or(0);
        pri_b.cmp(&pri_a)
    });

    info!("[PATHFIND] Processing {} unclaimed jobs", unclaimed.len());

    let locked_dorfs = job_queue.get_locked_dorfs();
    if !locked_dorfs.is_empty() {
        info!("[PATHFIND] Locked dorfs: {:?}", locked_dorfs.len());
    }

    // Track dorfs assigned jobs in this frame to prevent multiple assignments
    let mut assigned_this_frame: std::collections::HashSet<Entity> =
        std::collections::HashSet::new();

    for job_idx in unclaimed.into_iter().take(MAX_JOBS_PER_TICK) {
        let (kind, state, _) = match job_queue.get_job_at(job_idx) {
            Some(k) => k,
            None => continue,
        };

        if !matches!(state, JobState::Unclaimed) {
            info!(
                "[PATHFIND] Job idx={} state={:?} - skipping",
                job_idx, state
            );
            continue;
        }

        let kind = kind.clone();
        let target_pos = kind.target();
        let scope = job_queue.get_scope_for_job(job_idx);
        let retry_count = job_queue.get_job_retry_count(job_idx).unwrap_or(0);

        info!(
            "[PATHFIND] Job idx={} kind={:?} target={:?} scope={} retry_count={}",
            job_idx, kind, target_pos, scope, retry_count
        );

        // Collect all dorfs and their current task status for debugging
        let mut total_dorfs = 0;
        let mut locked_count = 0;
        let mut not_standable_count = 0;
        let mut busy_count = 0; // Truly busy: Active AND non-Idle

        let candidate_dorfs: Vec<(Entity, IVec3)> = dorf_query
            .iter_mut()
            .filter_map(|(entity, queue, state, transform, _)| {
                total_dorfs += 1;

                if locked_dorfs.contains(&entity) || assigned_this_frame.contains(&entity) {
                    locked_count += 1;
                    return None;
                }

                let pos = transform.translation.as_ivec3();
                if !tilemap.is_standable(pos) {
                    not_standable_count += 1;
                    info!(
                        "[PATHFIND] Dorf {:?} NOT STANDABLE at pos={:?} z_level={}",
                        entity,
                        pos,
                        pos.z / crate::constants::ITILE_SIZE
                    );
                    return None;
                }

                // A dorf is available if:
                // 1. Queue is empty, OR
                // 2. Current task is Idle (we can replace it)
                let current_task = queue.current();
                let is_idle = queue.is_empty()
                    || current_task
                        .map(|t| matches!(t, Task::Idle { .. }))
                        .unwrap_or(false);

                // A dorf is "busy" if they're Active AND doing non-Idle work
                // We can interrupt Idle tasks but not other tasks
                let state_val: &TaskState = &*state;
                let is_busy = matches!(state_val, TaskState::Active) && !is_idle;

                if is_busy {
                    busy_count += 1;
                    return None;
                }

                Some((entity, pos))
            })
            .collect();

        info!(
            "[PATHFIND] Dorfs: total={} locked={} not_standable={} busy={} candidates={}",
            total_dorfs,
            locked_count,
            not_standable_count,
            busy_count,
            candidate_dorfs.len()
        );

        if candidate_dorfs.is_empty() {
            info!(
                "[PATHFIND] No candidate dorfs for job idx={} - incrementing retry",
                job_idx
            );
            job_queue.increment_retry(job_idx);
            continue;
        }

        let mut candidate_dorfs = candidate_dorfs;
        candidate_dorfs.sort_by_key(|(_, pos)| {
            (pos.x - target_pos.x).abs()
                + (pos.y - target_pos.y).abs()
                + (pos.z - target_pos.z).abs()
        });

        let dorfs_to_try: Vec<_> = candidate_dorfs.into_iter().take(scope as usize).collect();

        info!(
            "[PATHFIND] Trying {} dorfs for job idx={}",
            dorfs_to_try.len(),
            job_idx
        );

        if dorfs_to_try.is_empty() {
            job_queue.increment_retry(job_idx);
            continue;
        }

        let dorf_entities: Vec<Entity> = dorfs_to_try.iter().map(|(e, _)| *e).collect();
        if !job_queue.set_job_calculating(job_idx, dorf_entities.clone()) {
            info!(
                "[PATHFIND] Failed to set jobCalculating for idx={}",
                job_idx
            );
            continue;
        }

        let mut best_result: Option<(Entity, IVec3, i32)> = None;

        match kind {
            JobKind::FellTree { trunk_pos } => {
                let approach_tiles = find_all_standable_adjacent(&trunk_pos, &tilemap);
                info!(
                    "[PATHFIND] FellTree at {:?}: found {} approach tiles",
                    trunk_pos,
                    approach_tiles.len()
                );

                if approach_tiles.is_empty() {
                    info!(
                        "[PATHFIND] No approach tiles for tree at {:?} - suspending job",
                        trunk_pos
                    );
                    job_queue.suspend_job(job_idx);
                    continue;
                }

                for (dorf_entity, dorf_pos) in &dorfs_to_try {
                    let mut best_for_dorf: Option<(IVec3, i32)> = None;

                    for approach_tile in &approach_tiles {
                        match calculate_traversal_distance(&tilemap, *dorf_pos, *approach_tile) {
                            Ok(distance) => match best_for_dorf {
                                None => best_for_dorf = Some((*approach_tile, distance)),
                                Some((_, best_dist)) if distance < best_dist => {
                                    best_for_dorf = Some((*approach_tile, distance));
                                }
                                _ => {}
                            },
                            Err(()) => {
                                // Path not found for this tile
                            }
                        }
                    }

                    if let Some((approach, dist)) = best_for_dorf {
                        info!(
                            "[PATHFIND] Dorf at {:?} can reach tree via {:?} (dist={})",
                            dorf_pos, approach, dist
                        );
                        match best_result {
                            None => best_result = Some((*dorf_entity, approach, dist)),
                            Some((_, _, best_dist)) if dist < best_dist => {
                                best_result = Some((*dorf_entity, approach, dist));
                            }
                            _ => {}
                        }
                    } else {
                        info!(
                            "[PATHFIND] Dorf at {:?} CANNOT reach tree at {:?}",
                            dorf_pos, trunk_pos
                        );
                    }
                }
            }
            JobKind::HaulCargo { cargo_pos, .. } => {
                info!("[PATHFIND] HaulCargo at {:?}", cargo_pos);

                for (dorf_entity, dorf_pos) in &dorfs_to_try {
                    match calculate_traversal_distance(&tilemap, *dorf_pos, cargo_pos) {
                        Ok(distance) => {
                            info!(
                                "[PATHFIND] Dorf at {:?} can reach cargo (dist={})",
                                dorf_pos, distance
                            );
                            match best_result {
                                None => best_result = Some((*dorf_entity, cargo_pos, distance)),
                                Some((_, _, best_dist)) if distance < best_dist => {
                                    best_result = Some((*dorf_entity, cargo_pos, distance));
                                }
                                _ => {}
                            }
                        }
                        Err(()) => {
                            info!(
                                "[PATHFIND] Dorf at {:?} CANNOT reach cargo at {:?}",
                                dorf_pos, cargo_pos
                            );
                        }
                    }
                }
            }
        }

        if let Some((dorf_entity, approach_target, dist)) = best_result {
            info!(
                "[PATHFIND] Best dorf {:?} with approach {:?} (dist={})",
                dorf_entity, approach_target, dist
            );

            if let Some(job_id) = job_queue.assign_job(job_idx, dorf_entity, approach_target) {
                if let Ok((_, mut queue, mut state, _, mut ambulatory)) =
                    dorf_query.get_mut(dorf_entity)
                {
                    let job_kind = match job_queue.get_job_kind_at(job_idx) {
                        Some(k) => k,
                        None => continue,
                    };
                    let task = match job_kind {
                        JobKind::FellTree { trunk_pos } => Task::ChopTree {
                            job_id,
                            trunk_pos,
                            chop_ticks: 120,
                            step: ChopStep::MovingToTree {
                                approach: Some(approach_target),
                            },
                        },
                        JobKind::HaulCargo {
                            cargo_entity,
                            cargo_pos,
                            dest,
                        } => Task::HaulCargo {
                            job_id,
                            cargo_entity,
                            cargo_pos,
                            dest,
                            step: HaulStep::MovingToCargo {
                                approach: Some(approach_target),
                            },
                        },
                    };

                    queue.clear();
                    queue.push(task);
                    *state = TaskState::Pending;
                    ambulatory.current_path = None;
                    ambulatory.path_index = 0;
                    ambulatory.target = None; // Clear old target from previous task

                    info!(
                        "[PATHFIND] SUCCESS: Assigned job {:?} to dorf {:?} with approach {:?}",
                        job_id, dorf_entity, approach_target
                    );
                    assigned_this_frame.insert(dorf_entity);
                }
            }
        } else {
            info!("[PATHFIND] NO PATH FOUND for any dorf - retry_count will increment");
            job_queue.increment_retry(job_idx);
            for entity in &dorf_entities {
                job_queue.unclaim_jobs_for_entity(*entity);
            }
            let retry_count = job_queue.get_job_retry_count(job_idx).unwrap_or(0);
            if retry_count >= 10 {
                info!(
                    "[PATHFIND] Job idx={} suspended after {} retries",
                    job_idx, retry_count
                );
                job_queue.suspend_job(job_idx);
            }
        }
    }
}

fn find_all_standable_adjacent(trunk_pos: &IVec3, tilemap: &TileMap) -> SmallVec<[IVec3; 8]> {
    use crate::constants::ITILE_SIZE;
    let mut tiles = SmallVec::new();
    let standing_z = trunk_pos.z;

    for dx in -1i32..=1 {
        for dy in -1i32..=1 {
            if dx == 0 && dy == 0 {
                continue;
            }
            let candidate = IVec3::new(
                trunk_pos.x + dx * ITILE_SIZE,
                trunk_pos.y + dy * ITILE_SIZE,
                standing_z,
            );

            if tilemap.is_standable(candidate) {
                tiles.push(candidate);
            }
        }
    }
    tiles
}