use std::f64::consts::PI;
use crate::mesh::{Vec3, AABB};
use super::oracle::SdfOracle;
use super::state::{Crawler, CrawlerEvent, Phase};
use super::{CrawlerConfig, SurfaceHit};

/// Manages the lifecycle of all crawlers and collects surface samples.
pub struct CrawlerScheduler {
    pub crawlers: Vec<Crawler>,
    pub contacts: Vec<Vec3>,
    pub samples: Vec<SurfaceHit>,
    pub config: CrawlerConfig,
    pub bounds: AABB,
    next_id: u32,
    next_face: u32,
}

impl CrawlerScheduler {
    pub fn new(bounds: AABB, config: CrawlerConfig) -> Self {
        Self {
            crawlers: Vec::new(),
            contacts: Vec::new(),
            samples: Vec::new(),
            config,
            bounds,
            next_id: 0,
            next_face: 1,
        }
    }

    /// Phase 1: Deploy initial probes uniformly on the bounding sphere.
    pub fn deploy_probes(&mut self) {
        let center = self.bounds.center();
        let radius = self.bounds.diagonal() * 0.6;
        let n = self.config.initial_probes;

        // Fibonacci sphere for uniform distribution
        let golden = (1.0 + 5.0_f64.sqrt()) / 2.0;
        for i in 0..n {
            let theta = (2.0 * PI * i as f64) / golden;
            let phi = (1.0 - 2.0 * (i as f64 + 0.5) / n as f64).acos();

            let dir = Vec3::new(
                phi.sin() * theta.cos(),
                phi.sin() * theta.sin(),
                phi.cos(),
            );
            let pos = center + dir * radius;
            let inward = (center - pos).normalized();

            let id = self.next_id;
            self.next_id += 1;
            self.crawlers.push(Crawler::new_probe(id, pos, inward));
        }
    }

    /// Phase 2: Deploy patrol crawlers from contact points.
    pub fn deploy_patrol(&mut self) {
        let contacts: Vec<(Vec3, Vec3)> = self.crawlers.iter()
            .filter(|c| c.phase == Phase::Done && c.normal.length() > 0.5)
            .map(|c| (c.position, c.normal))
            .collect();

        for (pos, normal) in contacts {
            let tangent_basis = tangent_frame(normal);
            let per_contact = self.config.crawlers_per_contact;
            for i in 0..per_contact {
                let angle = 2.0 * PI * i as f64 / per_contact as f64;
                let dir = tangent_basis.0 * angle.cos() + tangent_basis.1 * angle.sin();

                let id = self.next_id;
                self.next_id += 1;
                let mut c = Crawler::new_probe(id, pos, dir);
                c.phase = Phase::Patrol;
                c.normal = normal;
                self.crawlers.push(c);
            }
        }
    }

    /// Advance all active crawlers by one step.
    /// Returns events generated this tick.
    pub fn step(&mut self, oracle: &dyn SdfOracle) -> Vec<CrawlerEvent> {
        let mut events = Vec::new();
        let step_size = self.bounds.diagonal() * self.config.step_fraction;
        let eps = self.config.surface_epsilon;
        let max_steps = self.config.max_steps;

        for crawler in &mut self.crawlers {
            if !crawler.is_active() { continue; }
            crawler.steps += 1;
            if crawler.steps > max_steps {
                crawler.phase = Phase::Done;
                events.push(CrawlerEvent::Completed { crawler_id: crawler.id });
                continue;
            }

            match crawler.phase {
                Phase::Contact => {
                    step_contact(crawler, oracle, step_size, eps, &mut events);
                }
                Phase::Patrol => {
                    step_patrol(crawler, oracle, step_size, eps, &mut events);
                }
                Phase::Spiral => {
                    step_spiral(crawler, oracle, step_size, eps, &mut events);
                }
                Phase::Scan => {
                    step_scan(crawler, oracle, step_size, eps, &mut events);
                }
                Phase::Done => {}
            }
        }

        // Detect path crossings between patrol crawlers
        let patrol: Vec<(usize, Vec3)> = self.crawlers.iter().enumerate()
            .filter(|(_, c)| c.phase == Phase::Patrol)
            .map(|(i, c)| (i, c.position))
            .collect();

        let cross_dist = step_size * 3.0;
        let mut crossings = Vec::new();
        for i in 0..patrol.len() {
            for j in (i + 1)..patrol.len() {
                let (ai, ap) = patrol[i];
                let (bi, bp) = patrol[j];
                if self.crawlers[ai].id == self.crawlers[bi].id { continue; }
                let dist = (ap - bp).length();
                if dist < cross_dist {
                    crossings.push((ai, bi, (ap + bp) * 0.5));
                }
            }
        }

        for (ai, bi, midpoint) in crossings {
            let id_a = self.crawlers[ai].id;
            let id_b = self.crawlers[bi].id;
            events.push(CrawlerEvent::PathCrossing {
                crawler_a: id_a,
                crawler_b: id_b,
                position: midpoint,
            });
            // Transition both to spiral to map face boundaries
            self.crawlers[ai].phase = Phase::Spiral;
            self.crawlers[ai].spiral_r = step_size;
            self.crawlers[ai].spiral_theta = 0.0;
            self.crawlers[bi].phase = Phase::Spiral;
            self.crawlers[bi].spiral_r = step_size;
            self.crawlers[bi].spiral_theta = PI; // opposite direction
        }

        events
    }

    /// Collect samples from scanners and assign face IDs.
    pub fn collect_samples(&mut self) {
        for crawler in &self.crawlers {
            if crawler.phase == Phase::Scan || crawler.phase == Phase::Done {
                // Samples are accumulated during step_scan via events
            }
        }
    }

    /// Assign idle (Done) crawlers to remaining unscanned faces.
    pub fn reassign_idle(&mut self, oracle: &dyn SdfOracle) {
        let scan_spacing = self.bounds.diagonal() * self.config.scan_spacing;

        // Find spiraling crawlers that have identified face regions
        let spiral_done: Vec<usize> = self.crawlers.iter().enumerate()
            .filter(|(_, c)| c.phase == Phase::Spiral && c.steps > 200)
            .map(|(i, _)| i)
            .collect();

        for idx in spiral_done {
            let face = self.next_face;
            self.next_face += 1;

            self.crawlers[idx].phase = Phase::Scan;
            self.crawlers[idx].face_id = face;
            self.crawlers[idx].scan_row = 0;
            self.crawlers[idx].scan_row_progress = 0.0;
            self.crawlers[idx].scan_row_length = scan_spacing * 100.0;
            self.crawlers[idx].scan_origin = self.crawlers[idx].position;

            // Scan direction: perpendicular to current direction on the surface
            let n = self.crawlers[idx].normal;
            let d = self.crawlers[idx].direction;
            self.crawlers[idx].scan_dir = n.cross(d).normalized();
        }

        // Reassign fully done crawlers
        let done_ids: Vec<usize> = self.crawlers.iter().enumerate()
            .filter(|(_, c)| c.phase == Phase::Done)
            .map(|(i, _)| i)
            .collect();

        let active_faces: Vec<(Vec3, Vec3, u32)> = self.crawlers.iter()
            .filter(|c| c.phase == Phase::Scan)
            .map(|c| (c.position, c.normal, c.face_id))
            .collect();

        // No work left if no active faces need help
        if active_faces.is_empty() { return; }

        let _ = oracle; // will use for surface projection in more advanced reassignment
        for &idx in &done_ids {
            // Reassign to a random active face (simple round-robin)
            let face_idx = idx % active_faces.len();
            let (pos, normal, face_id) = active_faces[face_idx];
            self.crawlers[idx].phase = Phase::Scan;
            self.crawlers[idx].position = pos;
            self.crawlers[idx].normal = normal;
            self.crawlers[idx].face_id = face_id;
            self.crawlers[idx].scan_row = 0;
            self.crawlers[idx].scan_row_progress = 0.0;
        }
    }

    /// Run the full pipeline until all crawlers are done.
    pub fn run(&mut self, oracle: &dyn SdfOracle) -> Vec<SurfaceHit> {
        // Phase 1: contact
        self.deploy_probes();
        loop {
            let events = self.step(oracle);
            let active = self.crawlers.iter().any(|c| c.phase == Phase::Contact);
            self.process_events(&events);
            if !active { break; }
        }

        // Phase 2: patrol
        self.deploy_patrol();
        for _ in 0..self.config.max_steps {
            let events = self.step(oracle);
            self.process_events(&events);
            let active = self.crawlers.iter()
                .any(|c| c.phase == Phase::Patrol || c.phase == Phase::Spiral);
            if !active { break; }
            // Periodically reassign
            self.reassign_idle(oracle);
        }

        // Phase 3: scan whatever we've identified
        for _ in 0..self.config.max_steps {
            let events = self.step(oracle);
            self.process_events(&events);
            self.reassign_idle(oracle);
            let active = self.crawlers.iter().any(|c| c.is_active());
            if !active { break; }
        }

        std::mem::take(&mut self.samples)
    }

    fn process_events(&mut self, events: &[CrawlerEvent]) {
        for event in events {
            match event {
                CrawlerEvent::ContactMade { position, normal, .. } => {
                    self.contacts.push(*position);
                    self.samples.push(SurfaceHit {
                        position: *position,
                        normal: *normal,
                        face_id: 0,
                    });
                }
                CrawlerEvent::SampleRecorded { position, normal, face_id, .. } => {
                    self.samples.push(SurfaceHit {
                        position: *position,
                        normal: *normal,
                        face_id: *face_id,
                    });
                }
                _ => {}
            }
        }
    }
}

// === Individual step functions ===

fn step_contact(
    c: &mut Crawler,
    oracle: &dyn SdfOracle,
    step_size: f64,
    eps: f64,
    events: &mut Vec<CrawlerEvent>,
) {
    let d = oracle.sdf(c.position);
    if d.abs() < eps {
        c.normal = oracle.gradient(c.position);
        c.phase = Phase::Done;
        events.push(CrawlerEvent::ContactMade {
            crawler_id: c.id,
            position: c.position,
            normal: c.normal,
        });
        return;
    }
    // Sphere-trace: step by SDF distance (clamped)
    let advance = d.abs().min(step_size);
    c.position = c.position + c.direction * advance;
}

fn step_patrol(
    c: &mut Crawler,
    oracle: &dyn SdfOracle,
    step_size: f64,
    _eps: f64,
    events: &mut Vec<CrawlerEvent>,
) {
    // Move along surface: step in direction, then project back
    let candidate = c.position + c.direction * step_size;
    if let Some((proj, normal)) = oracle.project_to_surface(candidate, 16) {
        let normal_change = 1.0 - c.normal.dot(normal).abs();
        if normal_change > 0.3 {
            events.push(CrawlerEvent::FaceBoundary {
                crawler_id: c.id,
                position: proj,
                normal_change,
            });
        }
        // Update direction to stay tangent
        let raw_dir = (proj - c.position).normalized();
        let tangent = (raw_dir - normal * raw_dir.dot(normal)).normalized();
        c.position = proj;
        c.normal = normal;
        if tangent.length() > 0.5 {
            c.direction = tangent;
        }
    } else {
        // Lost the surface — done
        c.phase = Phase::Done;
        events.push(CrawlerEvent::Completed { crawler_id: c.id });
    }
}

fn step_spiral(
    c: &mut Crawler,
    oracle: &dyn SdfOracle,
    step_size: f64,
    eps: f64,
    events: &mut Vec<CrawlerEvent>,
) {
    // Archimedean spiral on the surface
    c.spiral_theta += step_size / c.spiral_r.max(step_size);
    c.spiral_r += step_size * 0.05; // slow expansion

    let (t1, t2) = tangent_frame(c.normal);
    let offset = t1 * (c.spiral_r * c.spiral_theta.cos())
               + t2 * (c.spiral_r * c.spiral_theta.sin());
    let candidate = c.scan_origin + offset;

    if let Some((proj, normal)) = oracle.project_to_surface(candidate, 16) {
        let normal_change = 1.0 - c.normal.dot(normal).abs();
        c.position = proj;
        c.normal = normal;

        events.push(CrawlerEvent::SampleRecorded {
            crawler_id: c.id,
            position: proj,
            normal,
            face_id: c.face_id,
        });

        // If normal changes significantly, we've hit a face boundary
        if normal_change > 0.3 {
            events.push(CrawlerEvent::FaceBoundary {
                crawler_id: c.id,
                position: proj,
                normal_change,
            });
        }
    } else {
        c.phase = Phase::Done;
        events.push(CrawlerEvent::Completed { crawler_id: c.id });
    }

    let _ = eps;
}

fn step_scan(
    c: &mut Crawler,
    oracle: &dyn SdfOracle,
    step_size: f64,
    eps: f64,
    events: &mut Vec<CrawlerEvent>,
) {
    // Raster scan: move along scan_dir, step rows perpendicular
    c.scan_row_progress += step_size;

    let row_dir = if c.scan_row % 2 == 0 { c.scan_dir } else { -c.scan_dir };
    let row_offset = c.direction * (c.scan_row as f64 * step_size * 5.0);
    let candidate = c.scan_origin + row_offset + row_dir * c.scan_row_progress;

    if let Some((proj, normal)) = oracle.project_to_surface(candidate, 16) {
        c.position = proj;
        c.normal = normal;
        events.push(CrawlerEvent::SampleRecorded {
            crawler_id: c.id,
            position: proj,
            normal,
            face_id: c.face_id,
        });
    }

    if c.scan_row_progress > c.scan_row_length {
        c.scan_row += 1;
        c.scan_row_progress = 0.0;
        // Arbitrary limit on rows
        if c.scan_row > 200 {
            c.phase = Phase::Done;
            events.push(CrawlerEvent::Completed { crawler_id: c.id });
        }
    }

    let _ = eps;
}

/// Build an orthonormal tangent frame from a normal vector.
fn tangent_frame(n: Vec3) -> (Vec3, Vec3) {
    let up = if n.z.abs() < 0.9 {
        Vec3::new(0.0, 0.0, 1.0)
    } else {
        Vec3::new(1.0, 0.0, 0.0)
    };
    let t1 = n.cross(up).normalized();
    let t2 = n.cross(t1).normalized();
    (t1, t2)
}
