use nalgebra::{DVector, RowDVector};

use crate::{
    algebra::{distance_field::DistanceField, subspace::Line, Point, Scalar, Vector, N},
    particle_system::ParticleSystem,
};

use super::Constraint;

pub struct SliderConstraint {
    pub particle_id: usize,
    pub line: Line,
}

impl ParticleSystem {
    pub fn add_slider_constraint(&mut self, particle_id: usize, direction: Vector) {
        let point = self.particles[particle_id].position;
        self.constraints.push(Box::new(SliderConstraint {
            particle_id,
            line: Line::new(point, [direction]),
        }));
    }
}

impl Constraint for SliderConstraint {
    fn get_particles(&self) -> Vec<usize> {
        vec![self.particle_id]
    }

    fn c(&self, q: &DVector<Scalar>) -> Scalar {
        let position = q.fixed_rows::<N>(self.particle_id * N);
        let point = Point::from_coordinates(position.into());
        self.line.distance(point)
    }

    fn jacobian(&self, q: &DVector<Scalar>) -> RowDVector<Scalar> {
        let position = q.fixed_rows::<N>(self.particle_id * N);
        let point = Point::from_coordinates(position.into());
        let mut result = RowDVector::zeros(q.len());

        match (point - self.line.project_point(point)).try_normalize(0.0) {
            Some(normal) => {
                for i in 0..N {
                    result[self.particle_id * N + i] = normal[i]
                }
            }
            None => ()
        };


        result
    }
}

#[cfg(test)]
mod tests {
    use std::{fs::File, io::Write};

    use crate::{algebra::{Point, Vector}, constraint::{DAMPING_CONSTANT, SPRING_CONSTANT}, force::gravity::Gravity, particle_system::{Particle, ParticleSystem}, solver::Solver};

    #[test]
    fn test_drift_resiliance() {
        let origin = Point::origin();
        let mut system = ParticleSystem {
            particles: vec![Particle::new(origin, 1.0)],
            constraints: vec![],
            forces: vec![Box::new(Gravity {
                vector: Vector::new(-5.0, -1.0, 20.0),

            })],
            t: 0.0,
        };
        system.add_slider_constraint(0, Vector::x());

        let mut errors = vec![];
        let mut positions = vec![];

        let sim_time = 10.0;

        let dt = 1e-5;
        for i in 0..(sim_time / dt) as usize {
            system.apply_forces();
            system.enforce_constraints();
            system.step(dt);

            let (q, _) = system.collect_q();
            let error = system.constraints[0].c(&q);
            errors.push(error);
            positions.push(system.particles[0].position);
            // println!("Iteration {}, error = {}", i, error);
        }

        let error_max = errors.clone().into_iter().reduce(f64::max).unwrap();

        let mut error_sorted = errors.clone();
        error_sorted.sort_by(f64::total_cmp);
        let mean = error_sorted[errors.len() / 2];

        {
            let filename = format!("./plots/constraint/errors-s{}-d{}.txt", SPRING_CONSTANT, DAMPING_CONSTANT);
            let mut f = File::create(filename).unwrap();
            let s = errors.iter().fold(String::new(), |acc, e| acc + &e.to_string() + "\n");
            f.write(s.as_bytes()).unwrap();
        }
        {
            let filename = format!("./plots/slider/pos-s{}-d{}.txt", SPRING_CONSTANT, DAMPING_CONSTANT);
            let mut f = File::create(filename).unwrap();
            let s = positions.iter().fold(String::new(), |acc, e| acc + &e.y.to_string() + " " + &e.z.to_string() + "\n");
            f.write(s.as_bytes()).unwrap();
        }
        // assert!(error < error_max, "Error must decrease over time");
        assert!(error_max < 1.0e-5, "Error too high: MAX = {}, MEAN = {}", error_max, mean);
    }
}