class Pendulum {

    X: Vector[] = [];
    V: Vector[] = [];

    size: number;

    constructor(readonly M: number[], readonly L: number[], readonly color: p5.Color) {
        console.assert(M.length === L.length, M, L, "Masses and Lengths are not of equal length!");

        this.size = M.length;

        let currentPosition = new Vector(0, 0);
        for (let i = 0; i < this.size; i++){
            let a = Math.sqrt(L[i] * L[i] / 2);
            currentPosition.addC(-L[i], 0);
            this.X.push(currentPosition.copy());
            this.V.push(new Vector(0, 0));
        }
    }

    // using position based dynamics
    update(h: number) {
        const subSteps = 50;

        h /= subSteps;

        for (let k = 0; k < subSteps; k++) {

            let previousP = new Vector(0, 0);

            for (let i = 0; i < this.size; i++) {
                // apply external force (gravity)
                this.V[i].addC(0, manager.gravity * h * 50);

                // euler step
                let currentP = Vector.Add(this.X[i],Vector.Mult(this.V[i], h));

                // solve distance constraint
                let w1 = i === 0 ? 0 : 1 / this.M[i - 1];
                let w2 = 1 / this.M[i];

                let s = Vector.Sub(previousP, currentP);
                let n = s.copy();
                n.normalize();
                let l = s.mag();

                let deltaP1 = Vector.Mult(n, -w1 / (w1 + w2) * (l - this.L[i]));
                let deltaP2 = Vector.Mult(n, w2 / (w1 + w2) * (l - this.L[i]));

                previousP.add(deltaP1);
                currentP.add(deltaP2);

                // integrate
                if (i > 0){
                    this.V[i - 1] = Vector.Mult(Vector.Sub(previousP, this.X[i - 1]), 1 / h);
                    this.X[i - 1] = previousP;
                }

                previousP = currentP;
            }

            this.V[this.size - 1] = Vector.Mult(Vector.Sub(previousP, this.X[this.size - 1]), 1 / h);
            this.X[this.size - 1] = previousP;
        }
    }

    draw(){
        p.push();

        p.stroke(this.color);
        p.strokeWeight(2);
        p.fill(255);

        let p1 = new Vector(0, 0);
        for (let p2 of this.X){
            p.line(p1.x, p1.y, p2.x, p2.y);
            p1 = p2.copy();
        }

        for (let p2 of this.X){
            p.ellipse(p2.x, p2.y, 10, 10);
        }

        p.pop();
    }

}