use crate::{
    algebra::{Point, Scalar, Vector},
    constraint::Constraint,
    force::Force,
};

#[derive(Debug)]
pub struct Particle {
    pub mass: Scalar,
    pub position: Point,
    pub velocity: Vector,

    /// Force accumulator
    pub force: Vector,
}

impl Particle {
    pub fn new(position: Point, mass: Scalar) -> Self {
        Self {
            mass,
            position,
            velocity: Vector::zeros(),
            force: Vector::zeros(),
        }
    }

    pub fn apply_force(&mut self, force: Vector) {
        self.force += force;
    }
    pub fn reset_force(&mut self) {
        self.force = Vector::zeros()
    }
}

// #[derive(Debug)]
pub struct ParticleSystem {
    pub particles: Vec<Particle>,
    pub constraints: Vec<Box<dyn Constraint>>,
    pub forces: Vec<Box<dyn Force>>,

    /// Simulation clock
    pub t: Scalar,
}

impl ParticleSystem {
    pub fn get_kinetic_energy(&self) -> Scalar {
        self.particles.iter().fold(0.0, |acc, p| {
            let velocity = p.velocity.norm();
            let energy = p.mass * velocity * velocity / 2.0;
            acc + energy
        })
    }
}