export class Sensor {
    constructor(robot, offsetAngle, offsetDistance) {
        this.type = "base";
        this.robot = robot
        this.value = null; // Last recorded value
        this.offsetAngle = offsetAngle;
        this.offsetDistance = offsetDistance;
        this.hitX = null;
        this.hitY = null;
        this.hitObject = null;
        this.angle = 0;
        this.hitpoint = { x: null, y: null };

    }

    updatePosition() {
        const robotAngle = this.robot.body.angle;
        const offsetAngleRad = this.offsetAngle * (Math.PI / 180);
        this.startX = this.robot.body.position.x + this.offsetDistance * Math.cos(robotAngle + offsetAngleRad);
        this.startY = this.robot.body.position.y + this.offsetDistance * Math.sin(robotAngle + offsetAngleRad);

    }

    read(robot, gameWorld) {
        throw new Error("read() must be implemented in subclasses");
    }

    draw(ctx) {
        // Default empty draw, can be overridden
    }
}

export class RaycastSensor extends Sensor {
    constructor(robot, offsetAngle, offsetDistance, angle, range) {
        super(robot, offsetAngle, offsetDistance);  // Call the parent class constructor
        this.type = "line";
        this.angle = angle;
        this.distance = null;
        this.range = range;
    }

    updatePosition() {
        const robotAngle = this.robot.body.angle;
        const offsetAngleRad = this.offsetAngle * (Math.PI / 180);
        this.startX = this.robot.body.position.x + this.offsetDistance * Math.cos(robotAngle + offsetAngleRad);
        this.startY = this.robot.body.position.y + this.offsetDistance * Math.sin(robotAngle + offsetAngleRad);
        const sensorAngle = robotAngle + (this.angle * (Math.PI / 180));
        this.endX = this.startX + Math.cos(sensorAngle) * this.range;
        this.endY = this.startY + Math.sin(sensorAngle) * this.range;
    }

    read(robot, gameWorld) {
        //console.log(robot);
        //console.log(gameWorld);
        this.updatePosition();

        const angleInRadians = (this.robot.direction + this.angle) * Math.PI / 180;
        const x = this.robot.x + Math.cos(angleInRadians) * this.range;
        const y = this.robot.y + Math.sin(angleInRadians) * this.range;

        // Ensure gameWorld is available and properly passed to the sensor
        let hitPos = gameWorld.rayCast(this.startX, this.startY, this.endX, this.endY, [this.robot.body]);
        if (hitPos != null) {
            this.hitX = hitPos.x;
            this.hitY = hitPos.y;
            this.endX = this.hitX;
            this.endY = this.hitY;
            this.hitpoint = { x: this.hitX, y: this.hitY };
            this.distance = Math.sqrt(Math.pow(this.hitX - this.startX, 2) + Math.pow(this.hitY - this.startY, 2));
        } else {
            this.hitX = null;
            this.hitY = null;

            this.hitpoint = { x: this.hitX, y: this.hitY };
            this.distance = Math.sqrt(Math.pow(this.endX - this.startX, 2) + Math.pow(this.endY - this.startY, 2));
        }
        //     console.log("Obstacle detected!");
        // } else {
        //     console.log("Clear path!");
        // }
    }

    draw(ctx) {


        ctx.strokeStyle = "lime";
        ctx.fillStyle = "green"
        ctx.beginPath();
        ctx.arc(this.startX, this.startY, 2, 0, 2 * Math.PI);
        ctx.fillStyle = "red";
        ctx.fill();
        ctx.stroke();

        if (this.hitX != null) {
            ctx.strokeStyle = "red";
            ctx.fillStyle = "red"
            ctx.beginPath();
            ctx.arc(this.hitX, this.hitY, 2, 0, 2 * Math.PI);
            ctx.fillStyle = "red";
            ctx.fill();
            ctx.stroke();
        }


        ctx.strokeStyle = "green";
        ctx.beginPath();
        ctx.lineWidth = 2;
        ctx.moveTo(this.startX, this.startY);
        ctx.lineTo(this.endX, this.endY);
        ctx.stroke();
    }
}


export class FloorColorSensor extends Sensor {
    constructor(robot, offsetAngle, offsetDistance) {
        super(robot, offsetAngle, offsetDistance); // No angle offset, directly below robot
        this.hasLine = false;
        this.type = "color";
        this.fill = "red";
    }

    read(robot, gameWorld) {
        this.updatePosition();

        this.value = gameWorld.getFloorColor(this.startX, this.startY);
        this.hasLine = this.value;
        if (this.value) {
            this.fill = "green";
        } else {
            this.fill = "red";
        }
        //console.log(this.value);
        return this.value;
    }

    draw(ctx) {
        this.updatePosition();

        ctx.lineWidth = 1;
        ctx.strokeStyle = this.fill;
        ctx.fillStyle = "green"
        ctx.beginPath();
        ctx.arc(this.startX, this.startY, 2, 0, 2 * Math.PI);
        ctx.fillStyle = this.fill;
        ctx.fill();
        ctx.stroke();


    }
}