// Theo Jansen's STRANDBEEST
// <http://strandbeest.com/>
// Code adapted from user heltonbiker at Stack Overflow
// <http://stackoverflow.com/questions/6573415/evolutionary-algorithm-for-the-theo-jansen-walking-mechanism>

function fmod(a, b) {
    if (a < 0) {
        return b - (-a) % b
    } else {
        return a % b
    }
}

function fromPoint(p, d, theta) {
    return p.add(Vector.create([Math.cos(theta), Math.sin(theta)]).x(d))
}

function radians(d) {
    return d * Math.PI / 180
}

// Return 2-dimensional vector cross product of p and q.
function cross2(p, q) {
    var P = p.elements
    var Q = q.elements
    return P[0] * Q[1] - P[1] * Q[0]
}

// Return a point R that's distance l1 from p1, and distance l2 from p2,
// and p1-p2-R is clockwise.
function inter(p1, l1, p2, l2) {
    var D = p2.subtract(p1)                // Vector from p1 to p2.
    var d = D.modulus()                    // Dist from p2 to p1.
    var a = (l1*l1 - l2*l2 + d*d) / (2*d)  // Dist from p1 to radical line.
    var M = p1.add(D.x(a / d))             // Intersection of D w/radical line
    var h = Math.sqrt(l1*l1 - a*a)         // Distance from M to R1 or R2.
    var R = D.x(h / d)
    var r = Vector.create([-R.elements[1], R.elements[0]])
    // There are two results, but only one (the correct side of the
    // line) must be chosen
    var R1 = M.add(r)
    if (cross2(D, R1.subtract(p1)) < 0) {
        return M.subtract(r)
    } else {
        return R1
    }
}

function Beest() {
    this.angle = 0;
    this.lines = ["AC", "CD", "BD", "BE", "CE", "DF",
                  "BF", "FG", "EG", "GH", "EH"];
    this.magic = ["Bx", "By"].concat(this.lines);
    this.update();
}

Beest.prototype = {
    constructor: Beest,

    update: function () {
        var text = ""
        for (var i = 0; i < this.magic.length; ++i) {
            var m = this.magic[i]
            this[m] = parseFloat(params[m]);
        }
        this.footprint = [];
        this.linkageBroken = false;
        this.analyzedFootprint = false;
        this.tolerance = 2;       // Range of values of Y that count as "ground"
        this.Ymax = 0;
        this.liftheight = 35;
        this.lifttolerance = 15;
        this.maxliftheight = 60;
        this.maxlifttolerance = 20;
    },

    addPoint: function (label, p) {
        p.angle = this.angle
        p.label = label
        this.points.push(p)
        this[label] = p
    },

    footprintGrounded: function (i) {
        return (Math.abs(this.Ymax - this.footprint[i].elements[1])
                < this.tolerance)
    },
    
    footprintLifted: function (i) {
        return (Math.abs((this.Ymax - this.liftheight) - this.footprint[i].elements[1])
                < this.lifttolerance)
    },

    analyzeFootprint: function () {
        var f = this.footprint;
        this.Ymax = 0;            // Extremal value of Y: counts as "ground"
        this.Ymin = 1000000;
        for (var i = 0; i < f.length; ++i) {
            this.Ymax = Math.max(this.Ymax, f[i].elements[1]);
            this.Ymin = Math.min(this.Ymin, f[i].elements[1]);
        }
        var groundAngle = 0;     // Angle spent on the ground.
        var liftAngle = 0;
        var minVx = 1e10;
        var maxVx = -1e10;
        for (var i = 0; i < f.length; ++i) {
            if (this.footprintGrounded(i)) {
                var j = (i + 1) % f.length
                var a = f[j].angle
                var b = f[i].angle
                var dt
                if (a < b) {
                    dt = b - a
                } else {
                    dt = a - b - 360
                }
                groundAngle += dt
                if (dt > 0) {
                    var vx = (f[j].elements[0] - f[i].elements[0]) / dt
                    minVx = Math.min(minVx, vx)
                    maxVx = Math.max(maxVx, vx)
                }                
            }
            if (this.footprintLifted(i)) {
                var j = (i + 1) % f.length
                var a = f[j].angle
                var b = f[i].angle
                var dt
                if (a < b) {
                    dt = b - a
                } else {
                    dt = a - b - 360
                }
                liftAngle += dt
            }
        }
        this.analyzedFootprint = true

        var text = ""
        for (var i = 0; i < this.magic.length; ++i) {
            var m = this.magic[i]
            text += m + "=" + this[m] + "; "
        }
        text += "groundScore: " + (groundAngle / 360.0).toFixed(3);
        text += "; dragScore: " + (Math.max(- maxVx + minVx)).toFixed(3);
        text += "; liftScore: " + (liftAngle / 360.0).toFixed(3);
        var maxliftscore = (this.Ymax - this.Ymin - this.maxliftheight) / this.maxlifttolerance;
        if(maxliftscore < 0.0) {
            maxliftscore = 0.0;
        }
        //write(text);
        setFinished('ground', (groundAngle / 360.0), 'drag', (Math.max(- maxVx + minVx)), 'lift', (liftAngle / 360.0), 'maxlift', maxliftscore);
        isFinished = 1;
    },

    tick: function (dt) {
        this.angle += speed * dt;
        this.points = []
        this.addPoint("A", Vector.create([0,0]))
        this.addPoint("B", Vector.create([this.Bx, -this.By]))
        this.addPoint("C", fromPoint(this.A, this.AC, radians(this.angle)))
        this.addPoint("D", inter(this.C, this.CD, this.B, this.BD))
        this.addPoint("E", inter(this.B, this.BE, this.C, this.CE))
        this.addPoint("F", inter(this.D, this.DF, this.B, this.BF))
        this.addPoint("G", inter(this.F, this.FG, this.E, this.EG))
        this.addPoint("H", inter(this.G, this.GH, this.E, this.EH))
        if (isNaN(this.H.elements[0]) || isNaN(this.H.elements[1])) {
            this.linkageBroken = true;
            setFailed("Broken Linkage");
            isFinished = 1;
        } else {
            this.footprint.push(this.H)
        }
        var footprintComplete = false
        while (this.footprint[0].angle - 360 > this.angle) {
            this.footprint.shift()
            footprintComplete = true
        }
        if (!this.analyzedFootprint
            && !this.linkageBroken
            && footprintComplete) 
        {
            this.analyzeFootprint()
        }
    },

}

var speed = -60;                 // Speed of crank rotation, degrees/sec.
var lastFrame;
var beest;
var isFinished = 0;
function beestTick() {
    var t = (new Date()).getTime()
    var dt = Math.min(1.0 / 30, (t - lastFrame) / 1000.0)
    lastFrame = t

    beest.tick(params.dt)

}

lastFrame = (new Date()).getTime();
beest = new Beest();
while(!isFinished) {
    beestTick();
}