/* Lemmings - sensor code.
 *
 * 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/>.
 */
class Sensor {
    constructor(id, props) {
        Object.assign(this, {
            id,
            parent: null,
            lookAngle:0,
            attachRadius: simInfo.robotSize,
            value: null,
            valueStr: "<uninit>",
            minVal: 0,
            maxVal: 50,
            attachAngle: 0,
        }, props)
        this.info = this
        this.setAngle(this.attachAngle)
    }
    setAngle(attachAngle) {
        this.attachAngle = attachAngle
        Object.assign(this, Vec2.fromPolar(Vec2.zero, this.attachRadius, this.attachAngle))
    }
    plotSensor(context, x = this.x, y = this.y) {
        context.beginPath();
        context.arc(x, y, this.getWidth()/2, 0, 2*Math.PI);
        context.closePath();
        context.fillStyle = 'black';
        context.strokeStyle = this.color ? convrgb(this.color) : 'black';
        context.fill();
        context.stroke();
    }
    onDragging(sensor, event) {
        const {x, y} = sim.bay.transformMouse(event.mouse),
              mAngle = Math.atan2(y, x),
              mRadius = Math.sqrt(x*x + y*y);
        this.attachRadius = mRadius
        this.setAngle(mAngle)
        sim.bay.repaint()
    }
    getWidth() { return 2 }
    getHeight() { return 2 }
    mouseHit(x, y) {
        const mouse = sim.bay.transformMouse({x,y})
        return Vec2.distLess(this, mouse, this.getWidth()/2 + 1)
    }
};
class DistanceSensor extends Sensor {
    sensorRay(bodies, start, end) {
        // Cast ray of supplied length and return the bodies that collide with it.
        const rayLength = Vec2.dist(start, end)
        const rayAngle = Vec2.angle(Vec2.sub(end, start))
        const rayWidth = 1e-100,
              ray = Vec2.avg(start, end),
              rayRect = Matter.Bodies.rectangle(ray.x, ray.y, rayLength, rayWidth,
                                                {isSensor: true, isStatic: true,
                                                 angle: rayAngle, role: 'sensor'});

        return bodies.filter(body => {
            // coarse check on body boundaries, to increase performance:
            return Matter.Bounds.overlaps(body.bounds, rayRect.bounds) && 
                body.parts.some((part, pp) => {
                    // skip the first part, if it's not the only one
                    if (pp == 0 && body.parts.length > 1) return false
                    // finer, more costly check on actual geometry:
                    if (Matter.Bounds.overlaps(part.bounds, rayRect.bounds)) {
                        const collision = Matter.SAT.collides(part, rayRect);
                        if (collision.collided) {
                            return true
                        }
                    }
                })
        })
    }
    rayCast() {
        /* Distance sensor simulation based on ray casting. Called from sensor
         * object, returns nothing, updates a new reading into this.value.
         *
         * Idea: Cast a ray with a certain length from the sensor, and check
         *       via collision detection if objects intersect with the ray.
         *       To determine distance, run a Binary search on ray length.
         * Note: Sensor ray needs to ignore robot (parts), or start outside of it.
         *       The latter is easy with the current circular shape of the robots.
         * Note: Order of tests are optimized by starting with max ray length, and
         *       then only testing the maximal number of initially resulting objects.
         * Note: The sensor's "ray" could have any other (convex) shape;
         *       currently it's just a very thin rectangle.
         */

        var bodies = Matter.Composite.allBodies(sim.engine.world);
        if (this.filter) bodies = bodies.filter(this.filter)

        const robotAngle = this.parent.body.angle,
              rayAngle = robotAngle + this.attachAngle + this.lookAngle;

        const rPos = this.parent.body.position,
              rSize = this.attachRadius,
              startPoint = Vec2.fromPolar(rPos, this.attachRadius, robotAngle+this.attachAngle)

        function getEndpoint(rayLength) {
            return Vec2.fromPolar(startPoint, rayLength, rayAngle)
        };
        const sensorRay = (bodies, rayLength) => {
            return this.sensorRay(bodies, startPoint, getEndpoint(rayLength))
        };

        // call 1x with full length, and check all bodies in the world;
        // in subsequent calls, only check the bodies resulting here
        var rayLength = this.maxVal;
        bodies = sensorRay(bodies, rayLength);
        function binarySearch(lo, hi, cond) {
            let x = hi
            while (lo < x) {
                if (cond(x)) {
                    hi = x
                } else {
                    lo = x
                }
                x = Math.floor(lo + (hi-lo)/2)
            }
            return x
        }
        // if some collided, search for maximal ray length without collisions
        if (bodies.length > 0) {
            rayLength = binarySearch(0, rayLength, len => sensorRay(bodies, len).length > 0)
        }
        // increase length to (barely) touch closest body (if any)
        rayLength += 1;
        bodies = sensorRay(bodies, rayLength);
        return [bodies, rayLength, startPoint, getEndpoint(rayLength)]
    }
    sense() {
        const context = document.getElementById('arenaLemming').getContext('2d');
        const [bodies, rayLength, startPoint, endPoint] = this.rayCast()
        if (simInfo.debugSensors) {  // if invisible, check order of object drawing
            // draw the resulting ray
            context.strokeStyle = convrgb(this.parent.info.color)
            context.lineWidth = 0.5;
            drawVertices(context, [startPoint, endPoint])
            // mark all objects's lines intersecting with the ray
            bodies.forEach(({vertices}) => drawVertices(context, vertices))
        }
        let rl

        // indicate if the sensor exceeded its maximum length by returning infinity
        if (rayLength > this.maxVal) {
            rl = Infinity;
        }
        else {
            rl = Math.floor(rayLength + gaussNoise(3));
            rl = Matter.Common.clamp(rl, this.minVal, this.maxVal);
        }

        this.value = rl;
        this.valueStr = padnumber(this.value, 2);
    }
}
function clamp(x, min, max) {
    return x < min ? min : x > max ? max : x
}
class ColorSensor extends DistanceSensor {
    sense() {
        var bodies = Matter.Composite.allBodies(sim.engine.world);
        if (this.filter) bodies = bodies.filter(this.filter)

        const robotAngle = this.parent.body.angle,
              rayAngle = robotAngle + this.attachAngle + this.lookAngle;

        const rPos = this.parent.body.position,
              rSize = this.attachRadius,
              middle = Vec2.fromPolar(rPos, this.attachRadius + this.dist, robotAngle + this.attachAngle),
              startPoint = Vec2.fromPolar(middle, -this.width / 2, rayAngle + Math.PI/2),
              endPoint = Vec2.fromPolar(middle, this.width / 2, rayAngle + Math.PI/2)
        bodies = this.sensorRay(bodies, startPoint, endPoint)
        
        if (simInfo.debugSensors) {  // if invisible, check order of object drawing
            const context = document.getElementById('arenaLemming').getContext('2d');
            // draw the resulting ray
            context.strokeStyle = convrgb(this.parent.info.color)
            context.lineWidth = 0.5;
            drawVertices(context, [startPoint, endPoint])
            // mark all objects's lines intersecting with the ray
            bodies.forEach(({vertices}) => drawVertices(context, vertices))
        }
        let color
        if (bodies.length) {
            let r = 0
            let g = 0
            let b = 0
            let len = 0
            bodies.forEach(({color}) => {
                if (color) {
                    r += color[0]
                    g += color[1]
                    b += color[2]
                    len++
                }
            })
            if (len) color = [r/len,g/len,b/len]
            else color = [255,255,255]
        } else {
            color = [255,255,255]
        }
        color = color.map(x => Math.max(0, Math.min(x + gaussNoise(), 255)))
        this.value = color
        this.valueStr = `</span><span style="color: ${convrgb(color)}">` +
            color.map(x => format(x)).join(', ')
    }
}
class Gyroscope extends Sensor {
    sense() {
        this.value = (this.parent.body.angle * (180/Math.PI) + gaussNoise()) % 360
        this.valueStr = format(this.value) + '&deg;'
    }
}

class CollisionSensor extends DistanceSensor {
    constructor(id, props) {
        super(id, Object.assign({
            maxVal: 8
        }, props))
    }
    sense() {
        super.sense()
        this.value = this.value != Infinity
        this.valueStr = this.value ? "yes" : "no"
    }
}