use std::usize;

use nalgebra::{DVector, RowDVector};

use crate::{particle_system::{Scalar, N}, ParticleSystem, Point, Vector};

use super::Constraint;


pub struct SliderConstraint {
    pub particle_id: usize,

    pub point: Point,

    /// Has to be normalized
    pub dir: Vector,

    jacobian: RowDVector<Scalar>,
}

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,
                point,
                dir: direction.normalize(),
                jacobian: RowDVector::zeros(self.particles.len() * N),
            }
        ));

    }
}

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 d = position - self.point.coords;
        let lambda = d.dot(&self.dir);
        let distance_to_line = d - lambda * self.dir;

        distance_to_line.norm()
    }

    fn set_jacobian(&mut self, jacobian: RowDVector<Scalar>) {
        self.jacobian = jacobian
    }

    fn jacobian_prev(&self) -> RowDVector<Scalar> {
        self.jacobian.clone()
    }
}