/* Lemmings - robot and GUI script.
 *
 * Copyright 2016 Harmen de Weerd
 * Copyright 2017 Johannes Keyser, James Cooke, George Kachergis
 * Copyright 2017 Niels Boer, Tanja van Alfen, Yorick van Pelt 
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
/* 
The simulator was edited quite extensively to make it easier to work with.
The essential changes are the color sensor and gyroscope (in sensors.js) and the robot behavior
(in class Lemming in this file). Sensor placement was changed as well.
Other changes: object-orientation. es6. fixed the bay. consolidated drawing behavior,
svg pathseg workaround, robot debug display. Huge rewrites.
Feel free to use any of this code.
 */
// es6 modules are behind a flag in firefox.
// import { ColorSensor, DistanceSensor, Gyroscope } from './sensors.js'
"use strict"
// Simulation settings; please change anything that you think makes sense.
var simInfo = {
    maxSteps: 20000,  // maximal number of simulation steps to run
    airDrag: 0.1,  // "air" friction of enviroment; 0 is vacuum, 0.9 is molasses
    boxFric: 0.005, // friction between boxes during collisions
    boxMass: 0.01,  // mass of boxes
    boxSize: 15,  // size of the boxes, in pixels
    robotSize: 13,  // approximate robot radius, in pixels (note the SVG gets scaled down)
    robotMass: 0.4, // robot mass (a.u)
    gravity: 0,  // constant acceleration in Y-direction
    bayScale: 2,  // scale within 2nd, inset canvas showing robot in it's "bay"
    debugSensors: true,  // plot sensor rays and mark detected objects
    debugMouse: true,  // allow dragging any object with the mouse
    wanderRate: 0.0002,
};
class SVGRobot extends Robot {
    makeBody(robotInfo) {
        // load robot's body shape from SVG file
        const bodySVGpoints = loadFromSVG();
        return Matter.Bodies.fromVertices(robotInfo.init.x,
                                          robotInfo.init.y,
                                          bodySVGpoints, {
                                              frictionAir: simInfo.airDrag,
                                              mass: simInfo.robotMass,
                                              color: [255, 255, 255],
                                              role: 'robot'
                                          }, true);
    }
}
class HebbDidaBot extends Robot {
    constructor(props) {
        const angle = Math.PI/3.5
        super(Object.assign({
            sensors: [
                new CollisionSensor('collL', {
                    attachAngle: -angle
                }),
                new CollisionSensor('collR', {
                    attachAngle: angle
                }),
                new DistanceSensor('distL', {
                    attachAngle: -angle
                }),
                new DistanceSensor('distR', {
                    attachAngle: angle
                }),
            ].concat(props.sensors || [])
        }, props))
        this.display = document.createElement('canvas')
        this.display.width = this.display.height = 100
        this.display.style.border = '1px solid black'
        this.weights = [[Math.random()/10,Math.random()/10],[Math.random()/10,Math.random()/10]]
    }
    move() {
        const {distL, distR, collL, collR} = this.getSensors()
        // feed forward: perception
        // collision
        function feedforward(weights, activations, intrinsic) {
            return intrinsic.map((x, i) => {
                return x + sum(zip(weights[i], activations).map(([a,b]) => a*b))
            })
        }
        function hebb(weight, lrate, frate, act_src, act_dst) {
            const avg = sum(act_src) / act_src.length
            return weight.map((row, i) => row.map((wij, j) =>
                                                  wij + (lrate * (act_src[j] * act_dst[i]) - frate * avg * wij) / act_src.length
                                                 ))
        }
        // input: 
        const motorweights = [[0,1],[1,0]]
        const collisionAct = [+collL, +collR]
        const threshold = theta => x => x > theta
        const percact = [distL, distR].map(x => x == Infinity ? 0 : (50 - x)/50)
        const collact = [+collL, +collR]
        const collisionlayer = feedforward(this.weights, percact, collact)
        const motorLayer = feedforward(motorweights, collisionlayer, [0,0])
        this.weights = hebb(this.weights, simInfo.learningRate, simInfo.forgetRate, percact, collisionlayer)
        const [left, right] = motorLayer.map(threshold(0.5))
        // output: 2 neurons. if left is activated, turn left. if right is activated, turn right.
        // if both, go backwards. otherwise: forward
        if (left && !right) this.rotate(-0.003)
        else if (!left && right) this.rotate(+0.003)
        else if (left && right) this.drive(-2e-4)
        else this.drive(2e-4)
        this.layers = [percact, collisionlayer, motorLayer]
    }
    getDisplay() {
        return this.layers
    }
    updateDisplay() {
        const ctx = this.display.getContext('2d')
        if (!this.weights) return
        ctx.clearRect(0,0,100,100)
        this.weights.forEach((a, i) => {
            a.forEach((b, j) => {
                ctx.fillStyle = convrgb([b*128, b*128, b*128].map(x => 255-x))
                const [x,y] = [j*50,i*50]
                ctx.fillRect(x, y, x+50, y+50)
            })
        })
    }
}
// our own lemming implementation
class Lemming extends SVGRobot {
    constructor(props) {
        super(Object.assign({
            sensors: [
                // define another sensor
                new DistanceSensor('dist', {
                    attachAngle: -Math.PI/7,
                    color: [0, 150, 0], 
                    attachRadius: 20,
                    lookAngle: Math.PI/4,
                }),
                new Gyroscope('gyro', {
                    attachRadius: 5,
                    color: [100,100,0]
                }),
                new ColorSensor('carry', {
                    attachAngle: Math.PI/5,
                    lookAngle: -Math.PI/5,
                    color: [255, 100, 0],
                    attachRadius: 5,
                    dist: 5,
                    width: 15,
                }),
                // define another sensor
                new DistanceSensor('wall', {
                    attachAngle: -Math.PI/7,
                    color: [0, 150, 0], 
                    filter: x => x.role != 'box',
                    attachRadius: 20,
                    lookAngle: Math.PI/4,
                }),
            ].concat(props.sensors || [])
        }, props))
    }
    // turn this amount of degrees to a side 
    // exploit prototypal inheritance to temporarily override robot.move
    turnDeg(degs, cb) {
        if (Math.abs(degs) >= 320)
            return this.turnDeg(320, x => this.turnDeg(degs -320, cb))
        const torque = Math.sign(degs) * 0.002
        const start = this.getSensorValById('gyro')
        this.move = function() {
            const curAngle = this.getSensorValById('gyro')
            const turned = ((curAngle - start) * Math.sign(degs) + 360) % 360
            if (turned < 340 && (turned - Math.abs(degs) > 0)) {
                delete this.move
                if (cb) cb()
            } else {
                this.rotate(torque)
            }
        }
    }
    move() {
        // our lemming behaviour
        const {carry: [r,g,b], dist, wall} = this.getSensors()
        let block = 0
        if (r > (g+b)) {
            block = 'red'
        } else if (b > r+g) {
            block = 'blue'
        }
        this.block = block
        if (dist < wall - 5) {
            this.state = 'block'
            // if it senses a block
            if (!block) return this.drive(2e-4) // no block: drive towards block?
            if (block == 'blue') void 0; // if blue: ignore
            if (block == 'red') return this.turnDeg(-90) // if red: leave block
        } else if (wall < Infinity) {
            this.state = 'wall'
            // no block: turn left or right
            if (!block) return this.turnDeg(90 * (Math.random() < 0.5 ? -1 : 1))
            if (block == 'blue') return this.turnDeg(-90) // blue: leave
            if (block == 'red') return this.turnDeg(90) // red: keep
        } else {
            this.state = 'nothing'
        }
        // by default: wander
        this.rotate(simInfo.wanderRate);
        this.drive(0.0002);
    }
    getDisplay() {
        return "carrying " + (this.block || "no") + " block seeing " + this.state
    }
}
var sim = null
var resultsTable = null
let sim_id_counter = 0
class Simulation {
    constructor() {
        this.bay = null
        this.robots = null
        this.runner = null
        this.world = null
        this.engine = null
        this.curSteps = 0
        this.doContinue = false
        this.robots = []
        this.id = sim_id_counter++
    }
    init() {
        const arena = document.getElementById("arenaLemming"),
              {height, width} = arena
        this.elem = arena
        arena.style.backgroundColor = 'silver'
        Object.assign(this, {height, width})

        /* Create a MatterJS engine and world. */
        this.engine = Matter.Engine.create();
        this.world = this.engine.world;
        this.world.gravity.y = simInfo.gravity;
        this.engine.timing.timeScale = 1;

        /* Create walls and boxes, and add them to the world. */
        // note that "roles" are custom properties for rendering (not from MatterJS)
        function getWall(x, y, width, height) {
            return Matter.Bodies.rectangle(x, y, width, height, {
                isStatic: true, role: 'wall',
                color:[150, 150, 150]
            });
        };
        const wall_lo = getWall(width/2, height-5, width-5, 5),
              wall_hi = getWall(width/2, 5, width-5, 5),
              wall_le = getWall(5, height/2, 5, height-15),
              wall_ri = getWall(width-5, height/2, 5, height-15);
        Matter.World.add(this.world, [wall_lo, wall_hi, wall_le, wall_ri]);

        /* Add a bunch of boxes in a neat grid. */
        function getBox(x, y) {
            // flip coin for red vs blue and add rgb
            const color = (Math.random() < 0.5) ? [0, 0, 200] : [200, 0, 0]
            return Matter.Bodies.rectangle(x, y, simInfo.boxSize, simInfo.boxSize, {
                frictionAir: simInfo.airDrag,
                friction: simInfo.boxFric,
                mass: simInfo.boxMass,
                role: 'box',
                color
            });
        }
        const [nBoxX, nBoxY] = ["boxX", "boxY"].map(x => +document.getElementById(x).value)

        const gapX = 40, gapY = 30
        const startX = (width - (simInfo.boxSize * nBoxX + (nBoxX-1) * gapX)) / 2,
              startY = (height - (simInfo.boxSize * nBoxY + (nBoxY-1) * gapY)) / 2,
              stack = Matter.Composites.stack(startX, startY,
                                              nBoxX, nBoxY,
                                              gapX, gapY, getBox);
        Matter.World.add(this.world, stack);

        /* Add debug ging mouse control for dragging objects. */
        if (simInfo.debugMouse){
            const mouseConstraint = Matter.MouseConstraint.create(this.engine, {
                mouse: Matter.Mouse.create(arena),
                // spring stiffness mouse ~ object
                constraint: {stiffness: 0.5}
            });
            Matter.World.add(this.world, mouseConstraint);
        }
        // Add the tracker functions from mouse.js
        addMouseTracker(arena);

        /* Running the MatterJS physics engine (without rendering). */
        this.runner = Matter.Runner.create({fps: 60, isFixed: false});
        // register function simStep() as callback to MatterJS's engine events
        Matter.Events.on(this.engine, 'tick', this.step.bind(this));

        this.bay = new Bay()

    }
    destroy() {
        this.stop()
        Matter.Engine.clear(this.engine)
        this.elem.removeMouseTracker()
        this.bay.destroy()
    }
    step(draw = true) {
        // console.log("stepping", this.id)
        // advance simulation by one step (except MatterJS engine's physics)
        if (this.curSteps < simInfo.maxSteps) {
            if (draw) {
                this.bay.repaint();
                this.draw();
            }
            this.robots.forEach(robot => {
                robot.updateSensors();
                robot.move();
                // To enable selection by clicking (via mouse.js/graphics.js),
                // the position on the canvas needs to be defined in (x, y):
                const rSize = simInfo.robotSize;
                robot.x = robot.body.position.x - rSize;
                robot.y = robot.body.position.y - rSize;
            })
            // count and display number of steps
            this.curSteps += 1;
            if (draw) document.getElementById("SimStepLabel").innerHTML =
                padnumber(this.curSteps, 5) +
                ' of ' +
                padnumber(simInfo.maxSteps, 5);
        }
        else {
            this.stop()
        }
    }
    draw() {
        const context = this.elem.getContext('2d')
        context.clearRect(0, 0, this.width, this.height);
        // draw objects within world
        const Composite = Matter.Composite,
              bodies = Composite.allBodies(this.world);
        bodies.forEach(({role, vertices, color})  => {
            if (role == 'robot') return
            if (color) {
                context.strokeStyle = convrgb(color);
            }
            drawVertices(context, vertices);
        })
        context.lineWidth = 1;

        // draw all robots
        this.robots.forEach(robot => robot.plotRobot(context))
    }
    addRobot(robot) {
        this.robots.push(
            makeInteractiveElement(robot, this.elem))
        if (!this.bay.robot) this.bay.load(robot)
    }
    removeRobot(robot) {
        this.robots = this.robots.filter(x => x == robot)
        Matter.World.remove(this.world, robot.body)
    }
    start() {
        if (!this.doContinue) Matter.Runner.start(this.runner, this.engine);
        this.doContinue = true
    }
    stop()  {
        if (this.doContinue) Matter.Runner.stop(this.runner)
        this.doContinue = false
    }
    toggle() {
        if (this.doContinue) this.stop()
        else this.start()
    }
    runSteps(steps) {
        this.stop()
        const sensorDebug_ = simInfo.debugSensors
        simInfo.debugSensors = false
        for(let i = 0; i < steps-1; i++) {
            Matter.Engine.update(this.engine, 1000/60);
            this.step(false)
        }
        simInfo.debugSensors = sensorDebug_
        Matter.Engine.update(this.engine, 1000/60);
        this.step(true)
        
    }
}
function init() {  // called once when loading HTML file
    // the pathseg polyfill will run in the html document
    // but we need it in the svg. I changed the IIFE in to a
    // regular function and inject it into the svg using eval
    const svg = document.getElementById('robotbodySVG')
    svg.contentWindow.eval('(' + window.pathseg.toString() + ')()')
    resultsTable = new ResultsTable(document.getElementById("results"))
    resultsTable.clear()
    initSimulation()
    sim.start()
};
function initSimulation() {
    if (sim) {
        sim.stop()
        sim.destroy()
    }
    simInfo.boxFric = +document.getElementById("boxFric").value
    simInfo.learningRate = +document.getElementById("learningRate").value
    simInfo.forgetRate = +document.getElementById("forgetRate").value
    sim = new Simulation()
    sim.init()
    let noRobots = +document.getElementById('robotAmnt').value
    while(noRobots--) {
        sim.addRobot(new HebbDidaBot({
            // random color
            color: [Math.random()*255, Math.random()*255, Math.random()*255],  // color of the robot shape
            init: {x: 50*noRobots + 50, y: 50, angle: 0},  // initial position and orientation
        }))
    }
}
function promiseTimeout(time) {
    return new Promise((yes, no) => setTimeout(yes, time))
}
function blocksSorted(sim) {
    const {width, height} = sim
    return sim.world.composites[0].bodies.every(({position: {x, y}, color: [r, g, b]}) => {
        const dist = Math.min(x - 10, width - 10 - x, y - 10, height - 10 - y)
        // if red block: ignore
        if (r > g + b) return true
        // blue block
        if (b > r + g) {
            const dist = Math.min(x - 10, width - 10 - x, y - 10, height - 10 - y)
            // distance from wall
            return dist < 70
        } else {
            console.warn("unknown block type")
        }
    })
}
function redblocksatwall(sim) {
    const {width, height} = sim
    let result = 0
    sim.world.composites[0].bodies.forEach(({position: {x, y}, color: [r, g, b]}) => {
        const dist = Math.min(x - 10, width - 10 - x, y - 10, height - 10 - y)
        // if red block: count
        if (r > g + b) {
            if (dist < 70) result++
        }
    })
    return result
}
function countclusters(sim) {
    const boxes = sim.world.composites[0].bodies
    function dist(box1, box2) {
        return Vec2.dist(box1.position, box2.position)
    }
    const clusters = []
    const maxdist = 2 * Math.sqrt(2) * simInfo.boxSize
    const unclaimed = new Set(boxes)
    while(unclaimed.size) {
        const box = unclaimed.entries().next().value[0]
        const queue = new Set([box])
        let no_boxes = 0
        while(queue.size) {
            const box2 = queue.entries().next().value[0]
            unclaimed.delete(box2)
            queue.delete(box2)
            no_boxes++
            Array.from(unclaimed).filter(box3 => (dist(box2, box3) < maxdist)).forEach(x => queue.add(x))
        }
        clusters.push(no_boxes)
    }
    return clusters.sort().reverse()
}
// automatically run the experiments based on the values from the html
async function experimentRunner(stopCondition) {
    initSimulation()
    resultsTable.clearChart()
    while((stopCondition ? !stopCondition(sim) : true) && sim.curSteps < simInfo.maxSteps) {
        sim.runSteps(500)
        resultsTable.addWeightsPoint(sim.curSteps, sim.robots[0].weights)
        // take a break for the gui to stay responsive
        await promiseTimeout(50)
    }
    
    // save data
    const clusters = countclusters(sim)
    const rv =  {
        perc_in_clusters: (clusters.reduce((p,c) => c > 2 ? p+c : p, 0) / sum(clusters)) * 100,
        steps: sim.curSteps,
        clusters: countclusters(sim),
        robots: sim.robots.length
    }
    sim.destroy()
    sim = null
    return rv
}
// run a number of experiments
async function runExperiments() {
    let count = +document.getElementById('ffwdtimes').value
    while(count--) {
        await (experimentRunner().then(x => resultsTable.add(x)))
    }
}

class ResultsTable {
    constructor(elem) {
        this.elem = elem
        this.canvas = document.getElementById('weightschart')
        this.chart = new Chart(this.canvas.getContext('2d'), {
            type: 'line',
            data: {
                labels: [],
                datasets: [
                ]
            },
            options: {
                responsive: false
            },
        })
    }
    addWeightsPoint(step, weights) {
        this.chart.data.labels.push(step)
        this.chart.data.datasets[0].data.push(weights[0][0])
        this.chart.data.datasets[1].data.push(weights[0][1])
        this.chart.data.datasets[2].data.push(weights[1][0])
        this.chart.data.datasets[3].data.push(weights[1][1])
        this.chart.update()
    }
    clear() {
        this.elem.innerHTML = "<tr><th colspan=4>Experiment Results</th></tr>"
        const tr = document.createElement('tr')
        const headers = ["robots", "steps", "clusters", "cluster%"]
        headers.forEach(txt => {
            tr.appendChild(document.createElement('th')).appendChild(document.createTextNode(txt))
        })
        this.elem.appendChild(tr)
        this.clearChart()
    }
    clearChart() {
        this.chart.data.labels = []
        this.chart.data.datasets = [
            {label: "w00", data: [], fill: false, borderColor: 'red', backgroundColor: 'red'},
            {label: "w01", data: [], fill: false, borderColor: 'blue', backgroundColor: 'blue'},
            {label: "w10", data: [], fill: false, borderColor: 'green', backgroundColor: 'green'},
            {label: "w11", data: [], fill: false, borderColor: 'yellow', backgroundColor: 'yellow'},
        ]
        this.chart.update()
    }
    add({steps, clusters, robots, perc_in_clusters}) {
        const tr = document.createElement('tr')
        ;[robots, steps, clusters.toString(), perc_in_clusters].forEach(txt => {
            tr.appendChild(document.createElement('td')).appendChild(document.createTextNode(txt))
        })
        this.elem.appendChild(tr)
    }
}

class Bay {
    constructor() {
        this.elem = document.getElementById("bayLemming")
        this.context = this.elem.getContext('2d')
        this.center = {x: this.elem.width / simInfo.bayScale / 2,
                       y: this.elem.height / simInfo.bayScale / 2}
        this.robot = null
        this.elem.style.backgroundColor = 'silver'
        addMouseTracker(this.elem);
    }
    destroy() {
        this.elem.removeMouseTracker()
    }
    load(robot) {
        this.robot = robot
        robot.sensors.forEach(sensor => {
            makeInteractiveElement(sensor, this.elem)
        })
        const bayDisplay = document.getElementById('bayDisplay')
        bayDisplay.innerHTML = ""
        bayDisplay.appendChild(robot.display)
        this.repaint()
    }
    repaint() {
        const {context, robot, elem: robotBay} = this
        // update inset canvas showing information about selected robot
        context.clearRect(0, 0, this.elem.width, this.elem.height);
        if (!robot) return
        context.save()
        context.scale(simInfo.bayScale, simInfo.bayScale)
        context.translate(this.center.x, this.center.y)
        context.rotate(-Math.PI/2)
        robot.plotRobot(context, 0, 0, 0);
        context.restore()
        // print sensor values of selected robot next to canvas
        if (!(sim.curSteps % 5)) {  // update slow enough to read
            const sensorString = robot.sensors.map(({id, valueStr, color}) => {
                return `<br> <span style="color:${color ? convrgb(color) : 'black'}">id '${id}': ${valueStr}</span>`
            }).join('')
                                                  
            document.getElementById('SensorLabel').innerHTML = sensorString;
            robot.updateDisplay()
        }
    }
    transformMouse({x,y}) {
        // scale, translate, rotate by 90 degrees, 
        return { y: (x / simInfo.bayScale) - this.center.x,
                 x: -((y / simInfo.bayScale) - this.center.y) }
    }
    
}

function loadFromSVG() {
    var vertexSets = [];
    const svg = document.getElementById('robotbodySVG'),
          data = svg.contentDocument;

    for (const path of data.getElementsByTagName('path')) {
        const points = Matter.Svg.pathToVertices(path, 30);
        vertexSets.push(Matter.Vertices.scale(points, 0.2, 0.2));
    }

    return vertexSets;
};
function Robot(robotInfo) {
    this.body = this.makeBody(robotInfo)
    Matter.World.add(sim.world, this.body);
    Matter.Body.setAngle(this.body, robotInfo.init.angle);

    // instantiate its sensors
    this.sensors = robotInfo.sensors;
    this.sensors.forEach(sensor => sensor.parent = this)

    // attach its helper functions
    this.info = robotInfo;
    this.display = document.createElement('span')
}
Object.assign(Robot.prototype, {
    makeBody(robotInfo) {
        const nSides = 20,
              circle = Matter.Bodies.circle;
        return circle(robotInfo.init.x, robotInfo.init.y, simInfo.robotSize,
                      {
                          frictionAir: simInfo.airDrag,
                          mass: simInfo.robotMass,
                          color: [255, 255, 255],
                          role: 'robot'
                      }, nSides)
    },
    rotate(torque=0) {
        /* Apply a torque to the robot to rotate it.
         *
         * Parameters
         *   torque - rotational force to apply to the body.
         *            Try values around +/- 0.005.
         */
        this.body.torque = torque;
    },
    getDisplay() {
        return ""
    },
    updateDisplay() {
        this.display.innerHTML = this.getDisplay()
    },
    
    drive(force=0) {
        /* Apply a force to the robot to move it.
         *
         * Parameters
         *   force - force to apply to the body.
         *           Try values around +/- 0.0005.
         */
        const orientation = this.body.angle,
              force_vec = Matter.Vector.create(force, 0),
              move_vec = Matter.Vector.rotate(force_vec, orientation);
        Matter.Body.applyForce(this.body, this.body.position , move_vec);
    },
    updateSensors() {
        this.sensors.forEach(sensor => sensor.sense())
    },
    getSensorValById(id) {
        const sensor = this.sensors.find(sensor => sensor.id == id)
        return sensor ? sensor.value : undefined
    },
    getSensors() {
        return this.sensors.reduce((p,{id, value}) => (p[id] = value, p), {})
    },
    move() {
        // TODO: Define Lemming program here.
        const distL = this.getSensorValById('distL'),
              distR = this.getSensorValById('distR');
        
        //robot.rotate(+0.005);
        //robot.drive(0.0005);
    },
    mouseHit(x, y) {
        return Vec2.distLess(this.body.position, {x,y}, this.getWidth()/2 + 1)
    },
    getWidth() { return 2 * simInfo.robotSize },
    getHeight() { return 2 * simInfo.robotSize },
    onDrop(robot, event) {
        this.isDragged = false
    },
    onDrag(robot, event) {
        this.isDragged = true
        sim.bay.load(this)
        return true
    },
    plotRobot(context,
              x = this.body.position.x,
              y = this.body.position.y,
              angle = this.body.angle) {
        const showInternalEdges = false;
        const body = this.body

        // MatterJS thinks in world coords
        // translate the world canvas to compensate
        context.save();
        context.rotate(-body.angle + angle);
        context.translate(-body.position.x + x, -body.position.y + y);

        // handle compound parts
        context.beginPath();
        context.strokeStyle = convrgb(body.color);
        context.lineWidth = 1.5;
        body.parts.forEach(({vertices}, k) => {
            if (k == 0 && body.parts.length > 1) return
            context.moveTo(vertices[0].x, vertices[0].y);
            let fn, wasInternal = true
            vertices.forEach(fn = ({x,y, isInternal}, j) => {
                if (wasInternal) context.moveTo(x, y)
                else             context.lineTo(x, y)
                wasInternal = isInternal && !showInternalEdges
            })
            fn(vertices[0])
        })
        context.stroke()

        // to draw the rest, rotate & translate again
        context.restore()
        
        context.save();
        context.translate(x, y);
        context.rotate(angle);

        // Plot sensor positions into world canvas.
        this.sensors.forEach(sensor => sensor.plotSensor(context))
        context.restore();
    }
})


function padnumber(number, size) {
    if (number == 'Infinity') return 'inf'
    return (''+number).padStart(size, '0')
}

function format(number) {
    // prevent HTML elements to jump around at sign flips etc
    return (number >= 0 ? '+' : '−') + Math.abs(number).toFixed(1);
}

function toggleSimulation() {
    sim.toggle()
}