const g = 9.81

class NPendulum {

    pendula: Pendulum[] = []

    constructor(lengths, masses, startRad, color) {
        switch (lengths.length) {
            case 1:
                this.pendula.push(new Pendulum(lengths[0], masses[0], startRad, color));
                break;
            case 2:
                let p1 = new Pendulum(lengths[0], masses[0], startRad, color);
                let p2 = new Pendulum(lengths[1], masses[1], startRad, color);
                p1.calcAcc = function(pendula){
                    let p2 = pendula[1];
                    return -g / this.l * p.sin(this.rad) - p2.l * p2.m / this.l / (this.m + p2.m)
                        * (p.cos(this.rad - p2.rad) * p2.acc + p.sin(this.rad - p2.rad) * p.pow(p2.vel, 2));
                }
                p2.calcAcc = function (pendula){
                    let p1 = pendula[0];
                    return -g / this.l * p.sin(this.rad) - p1.l / this.l
                        * (p.cos(p1.rad - this.rad) * p1.acc - p.sin(p1.rad - this.rad) * p.pow(p1.vel, 2));
                }
                this.pendula.push(p1, p2);
                break;
        }
        this.pendula[0].origin = p.createVector(0, 0);
    }

    updateOrigins(){
        this.pendula.forEach((p, i) => {
            if (i > 0){
                let before = this.pendula[i - 1];
                p.origin = p5.Vector.add(before.origin, before.pos);
            }
        });
    }

    update(h){
        this.pendula.forEach((p, i) => {
            p.update(h, this.pendula);
        });
        this.updateOrigins();
    }

    draw(){
        this.pendula.forEach(p => p.draw());
    }

}


class Pendulum {

    l: number
    m: number

    acc: number = 0
    vel: number = 0
    rad: number

    origin: p5.Vector

    color: p5.Color


    constructor(l, m, rad, color) {
        this.l = l;
        this.m = m;
        this.rad = rad;
        this.color = color;
    }

    calcAcc(pendula){
        return -g / this.l * p.sin(this.rad);
    }

    update(h, pendula = []){
        this.acc = this.calcAcc(pendula);
        this.vel += this.acc * h;
        this.rad += this.vel * h;
    }

    draw(){
        let pos = this.pos;
        p.push();
        p.translate(this.origin);
        p.stroke(this.color);
        p.strokeWeight(3);
        p.line(0, 0, pos.x, pos.y);
        p.ellipse(pos.x, pos.y, this.m * 5, this.m * 5);
        p.pop();
    }

    get pos(){
        return p5.Vector.mult(p.createVector(p.sin(this.rad), p.cos(this.rad)), this.l);
    }

}