/* Lemmings - robot and GUI script.
 *
 * Copyright 2016 Harmen de Weerd
 * Copyright 2017 Johannes Keyser, James Cooke, George Kachergis, 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/>.
 */
"use strict"
// Simulation settings; please change anything that you think makes sense.
var simInfo = {
    maxSteps: 50000,  // 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
};
class Lemming extends Robot {
    constructor(props) {
        super(Object.assign({
            sensors: [
                new DistanceSensor('distR', {
                    attachAngle: Math.PI/(2.5),  // where the sensor is mounted on robot body
                    color: [150, 0, 0],  // sensor color [in RGB], to distinguish them
                    lookAngle: (Math.PI/7 - Math.PI/(2.5)),
                    attachRadius: 20
                }),
                // define another sensor
                new DistanceSensor('distL', {
                    attachAngle: -Math.PI/7,
                    color: [0, 150, 0], 
                    attachRadius: 20
                }),
                new Gyroscope('gyro', {
                    attachAngle: 0,
                    attachRadius: 5,
                    color: [100,100,0]
                }),
                new ColorSensor('carry', {
                    attachAngle: 0,
                    color: [255, 100, 0],
                    attachRadius: 5
                }),
                new DistanceSensor('wallR', {
                    attachAngle: Math.PI/2.5,  // where the sensor is mounted on robot body
                    color: [150, 0, 0],  // sensor color [in RGB], to distinguish them
                    filter: x => x.role == 'wall',
                    lookAngle: (Math.PI/7 - Math.PI/2.5),
                    attachRadius: 20,
                    
                }),
                // define another sensor
                new DistanceSensor('wallL', {
                    attachAngle: -Math.PI/7,
                    color: [0, 150, 0], 
                    filter: x => x.role == 'wall',
                    attachRadius: 20
                }),
            ].concat(props.sensors || [])
        }, props))
    }
    turnDeg(rads, cb) {
        if (Math.abs(rads) >= 320)
            return this.turnDeg(320, x => this.turnDeg(rads -320, cb))
        const torque = Math.sign(rads) * 0.01
        const start = this.getSensorValById('gyro')
        this.move = function() {
            const curAngle = this.getSensorValById('gyro')
            const turned = ((curAngle - start) * Math.sign(rads) + 360) % 360
            if (turned < 340 && (turned - Math.abs(rads) > 0)) {
                delete this.move
                if (cb) cb()
            } else {
                this.rotate(torque)
            }
        }
    }
    move() {
        //if (sim.curSteps % 250 == 0) this.turnDeg(-90)
        //return
        // TODO: Define Lemming program here.
        const vals = ['distL', 'distR', 'carry', 'wallL', 'wallR']
              .reduce(((p,c) => ((p[c] = this.getSensorValById(c)), p)), {})
        const [r,g,b] = vals.carry
        let block = 0
        if (r > (g+b)) {
            block = 'red'
        } else if (b > r+g) {
            block = 'blue'
        }
        const {distL, distR, wallL, wallR} = vals
        if (distL < wallL - 5 || distR < wallR - 5) {
            // 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 (wallL < Infinity || wallR < Infinity) {
            // 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
        }
        // by default: wander
        this.rotate(+0.002);
        this.drive(0.0002);
    }
}
var sim = null
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 = []
    }
    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 startX = 100, startY = 100,
              nBoxX = 5, nBoxY = 5,
              gapX = 40, gapY = 30,
              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()
    }
    step() {
        // advance simulation by one step (except MatterJS engine's physics)
        if (this.curSteps < simInfo.maxSteps) {
            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;
            document.getElementById("SimStepLabel").innerHTML =
                padnumber(this.curSteps, 5) +
                ' of ' +
                padnumber(simInfo.maxSteps, 5);
        }
        else {
            this.toggle()
        }
    }
    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()
    }
}
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() + ')()')
    sim = new Simulation()
    sim.init()
    sim.addRobot(new Lemming({
        color: [255, 255, 255],  // color of the robot shape
        init: {x: 50, y: 50, angle: 0},  // initial position and orientation
    }))
    sim.start()
};

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);
    }
    load(robot) {
        this.robot = robot
        robot.sensors.forEach(sensor => {
            makeInteractiveElement(sensor, this.elem)
        })
        // todo: removeinteractiveelement?
        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;
        }
    }
    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) {
    // load robot's body shape from SVG file
    const bodySVGpoints = loadFromSVG();
    this.body = Matter.Bodies.fromVertices(robotInfo.init.x,
                                           robotInfo.init.y,
                                           bodySVGpoints, {
                                               frictionAir: simInfo.airDrag,
                                               mass: simInfo.robotMass,
                                               color: [255, 255, 255],
                                               role: 'robot'
                                           }, true);

    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;
}
Object.assign(Robot.prototype, {
    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;
    },
    
    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
    },
    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()
}