// Ragdoll physics. Created by Reinder Nijhoff 2019
// @reindernijhoff
//
// https://turtletoy.net/turtle/736ec867cc
//

const turtle = new Turtle();

// standard verlet integration
class point {
    constructor(p, mass) {
        this.p = [...p];
        this.p_prev = [...p];
        this.mass = mass;
        this.acc = [0,0];
        this.links = [];
    }
    attach(p2, stiffness, draw) {
        this.links.push(new link(this, p2, stiffness, draw));
    }
    update(dt) {
        this.acc = [0, 9.8];
        let vel = sub2(this.p, this.p_prev);
        vel = scale2(vel, .999);
        const p = add2(add2(this.p, vel), scale2(this.acc, dt*dt/2));
        this.p_prev = this.p;
        this.p = p;
    }
    solveCollisions() {
        if (this.p[1] > 90) {
            this.p_prev[1] = this.p[1];
            this.p[1] = 90-(this.p[1]-90);
        }
    }
    solve() {
        this.solveCollisions();
        this.links.forEach(l => l.solve());
    }
    draw(t) {
        this.links.forEach(l => l.draw(t));
    }
}

class link {
    constructor(p1, p2, stiffness, draw) {
        this.p1 = p1;
        this.p2 = p2;
        this.stiffness = stiffness;
        this.visible = draw;
        this.distance = dist2(p1.p, p2.p);
    }
    solve() {
        const v = sub2(this.p1.p, this.p2.p);
        const d = length2(v);
        const diff = (this.distance - d)/d;
        const s = (1/this.p1.mass) / (1/this.p1.mass + 1/this.p2.mass);
        this.p1.p = add2(this.p1.p, scale2(v, s*this.stiffness*diff));
        this.p2.p = add2(this.p2.p, scale2(v, -(1-s)*this.stiffness*diff));
    }
    draw(t) {
        if (this.visible) {
            t.jump(this.p1.p);
            t.goto(this.p2.p);
        }
    }
}

class world {
    constructor() {
        this.points = [];
    }
    addPoint(p) {
        this.points.push(p);
        return p;
    }
    update(dt, relax) {
        this.points.forEach(p => p.update(dt));
        for (let i=0; i<relax; i++) {
            this.points.forEach(p => p.solve());
        }
    }
    draw(t) {
        this.points.forEach(p => p.draw(t));
    }
    random(s, o) {
        this.points.forEach(p => {
           p.p[0] += (Math.random() - .5) * s + o[0]; 
           p.p[1] += (Math.random() - .5) * s + o[1]; 
        });
    }
}

// ragdoll specific code
class circle extends point {
    constructor(p, mass, radius) {
        super(p, mass);
        this.radius = radius;
    }
    draw(t) {
        t.jump([this.p[0], this.p[1]-this.radius]);
        t.circle(this.radius);
        super.draw(t);
    }
    solveCollisions() {
        if (this.p[1] > 90-this.radius) {
            this.p_prev[1] = this.p[1];
            this.p[1] = 90-this.radius-(this.p[1]-90+this.radius);
        }
    }
}

class ragdoll {
    constructor(w, p, s) {
        const x = p[0], y = p[1];
        const head = w.addPoint(new circle([x,y-s/6], 5, s/8));
        const shoulder = w.addPoint(new point([x, y+s*.05], 20));
        shoulder.attach(head, 1, false);
        
        const elbowLeft = w.addPoint(new point([x-s/4,y], 2));
        elbowLeft.attach(shoulder, 1, true);
        const handLeft = w.addPoint(new point([x-s/2,y+s*.1], 2));
        handLeft.attach(elbowLeft, 1, true);

        const elbowRight = w.addPoint(new point([x+s/4,y], 2));
        elbowRight.attach(shoulder, 1, true);
        const handRight = w.addPoint(new point([x+s/2,y+s*.1], 2));
        handRight.attach(elbowRight, 1, true);

        const pelvis = w.addPoint(new point([x,y+s*.42], 15));
        pelvis.attach(shoulder, 1, true);

        const kneeLeft = w.addPoint(new point([x-s/6,y+s*.7], 10));
        kneeLeft.attach(pelvis, 1, true);
        const footLeft = w.addPoint(new point([x-s/6,y+s], 5));
        footLeft.attach(kneeLeft, 1, true);

        const kneeRight = w.addPoint(new point([x+s/6,y+s*.7], 10));
        kneeRight.attach(pelvis, 1, true);
        const footRight = w.addPoint(new point([x+s/6,y+s], 5));
        footRight.attach(kneeRight, 1, true);
        
        footLeft.attach(shoulder, 0.001, false);
        footRight.attach(shoulder, 0.001, false);

        // handRight.attach(pelvis, 0.001, false);
        // handLeft.attach(pelvis, 0.001, false);
        handLeft.attach(handRight, 0.0005, false);

        pelvis.attach(head, 0.01, false);
    }
}

const w = new world();

const r = new ragdoll(w, [-90,-90], 6);
w.random(.2, [.2,0]);

// The walk function will be called until it returns false.
function walk(i) {
    for (let j=0; j<40; j++) {
        w.update(1/60, 8);
    }
    w.draw(turtle);
    return i < 40;
}

// vec2 functions
function equal2(a,b) { return .001>dist_sqr2(a,b); }
function scale2(a,b) { return [a[0]*b,a[1]*b]; }
function add2(a,b) { return [a[0]+b[0],a[1]+b[1]]; }
function sub2(a,b) { return [a[0]-b[0],a[1]-b[1]]; }
function dot2(a,b) { return a[0]*b[0]+a[1]*b[1]; }
function dist_sqr2(a,b) { return (a[0]-b[0])*(a[0]-b[0])+(a[1]-b[1])*(a[1]-b[1]); }
function dist2(a,b) { return Math.sqrt(dist_sqr2(a,b)); }
function length2(a) { return Math.sqrt(dot2(a,a)); }
function segment_intersect2(a,b,d,c) {
    const e=(c[1]-d[1])*(b[0]-a[0])-(c[0]-d[0])*(b[1]-a[1]);
    if(0==e)return false;
    c=((c[0]-d[0])*(a[1]-d[1])-(c[1]-d[1])*(a[0]-d[0]))/e;
    d=((b[0]-a[0])*(a[1]-d[1])-(b[1]-a[1])*(a[0]-d[0]))/e;
    return 0<=c&&1>=c&&0<=d&&1>=d?[a[0]+c*(b[0]-a[0]),a[1]+c*(b[1]-a[1])]:false;
}