// Description of robot(s), and attached sensor(s) used by InstantiateRobot()
class Sensor {
    constructor(id, props) {
        Object.assign(this, {
            id,
            parent: null,
            lookAngle:0,
            attachRadius: simInfo.robotSize,
            value: null,
            valueStr: "<uninit>",
            minVal: 0,
            maxVal: 50
        }, 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 {
    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)
        };

        function sensorRay(bodies, rayLength) {
            // Cast ray of supplied length and return the bodies that collide with it.
            const rayWidth = 1e-100,
                  ray = Vec2.avg(startPoint, getEndpoint(rayLength)),
                  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
                            }
                        }
                    })
            })
        };

        // 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() {
        const [bodies, rayLength] = this.rayCast()
        let color
        if (bodies.length) {
            color = bodies[0].color.map(x => clamp(x + gaussNoise(), 0, 255))
        } else {
            color = [255,255,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;'
    }
}