import { RaycastSensor } from "./sensor.js";
import { FloorColorSensor } from "./sensor.js";

export class Robot {
    constructor(id, x, y, color = "blue") {
        this.id = id;
        this.x = x;
        this.y = y;
        this.velocity = 0;
        this.thrust = 0;
        this.angularThrust = 0
        this.width = 20;
        this.height = 20;
        this.color = color;
        this.sensors = [];

        this.hull = [
            { x: -this.width / 2, y: -this.height / 2 },   // Vertex 1
            { x: this.width / 2, y: -this.height / 2 },   // Vertex 2
            { x: this.width, y: 0 },   // Vertex 2
            { x: this.width / 2, y: this.height / 2 },   // Vertex 3
            { x: -this.width / 2, y: this.height / 2 }    // Vertex 4
        ];

        this.addSensor(new RaycastSensor(this, -40, 13, -45, 60));
        this.addSensor(new RaycastSensor(this, 40, 13, 45, 60));
        this.addSensor(new FloorColorSensor(this, 0, 0));


    }

    update(gameWorld) {
        let angle = this.body.angle;  // Get current rotation in radians

        let force = {
            x: Math.cos(angle) * this.thrust,
            y: Math.sin(angle) * this.thrust
        };

        Matter.Body.applyForce(this.body, this.body.position, force);
        Matter.Body.setAngularVelocity(this.body, this.angularThrust);
        //console.log(angle);
        this.update_sensors(gameWorld);
    }

    draw(ctx){
        this.draw(ctx);
    }

    update_sensors(gameWorld) {
        this.sensors.forEach(sensor => sensor.read(this, gameWorld));
    }

    addSensor(sensor) {
        if (this.sensors.length < 8) {
            this.sensors.push(sensor);
        }
    }

    update_position() {
        // const radians = (this.angle * Math.PI) / 180;
        // this.prevX = this.x;
        // this.prevY = this.y;
        // this.x += Math.cos(radians) * this.velocity;
        // this.y += Math.sin(radians) * this.velocity;


    }

    move(thrust) {
        this.thrust = thrust;
    }

    turn(degrees) {
        this.angularThrust += degrees;
    }

    draw(ctx) {
        
        // ctx.fillStyle = this.color;
        // ctx.save();
        // ctx.translate(this.x, this.y);
        // ctx.rotate((this.angle * Math.PI) / 180);

        // // Draw the rectangle (tank body)
        // ctx.fillRect(-this.width / 2, -this.height / 2, this.width, this.height);

        // // Draw the triangle (direction indicator)
        // ctx.strokeStyle = "black";
        // ctx.lineWidth = 2;
        // ctx.beginPath();
        // ctx.moveTo(this.width / 2, -this.height / 2);   // Tip of the triangle (front)
        // ctx.lineTo(this.width, 0);  // Bottom left of triangle
        // ctx.lineTo(this.width / 2, this.height / 2);   // Bottom right of triangle
        // ctx.closePath();

        // ctx.fill(); // Fill both the rectangle and the triangle

        // ctx.beginPath();
        // ctx.moveTo(this.hull[0].x, this.hull[0].y); // Start at the first vertex

        // // Loop through the rest of the vertices and draw lines
        // for (let i = 1; i < this.hull.length; i++) {
        //     ctx.lineTo(this.hull[i].x, this.hull[i].y);
        // }

        // // Close the path to form a closed polygon
        // ctx.closePath();

        // ctx.stroke(); // Draw the outline


        // ctx.restore();
        this.sensors.forEach(sensor => sensor.draw(ctx));
    }

    // Takes the polygon hull and updates it to local position and rotation.
    get_hull() {
        let angle = this.angle * (Math.PI / 180); // Convert degrees to radians
        //console.log(angle);
        this.transformedHull = this.hull.map(point => {
            let xRotated = point.x * Math.cos(angle) - point.y * Math.sin(angle);
            let yRotated = point.x * Math.sin(angle) + point.y * Math.cos(angle);

            return {
                x: this.body.position.x + xRotated, // Translate to robot's position
                y: this.body.position.y + yRotated
            };
        });
        //console.log(this.transformedHull      );
        return this.transformedHull;
    }

    draw_sensors(ctx) {
        ctx.fillStyle = "yellow";
        this.sensors.forEach(sensor => {
            const radians = ((this.angle + sensor.angleOffset) * Math.PI) / 180;
            const sensorX = this.body.position.x + Math.cos(radians) * 15;
            const sensorY = this.body.position.y + Math.sin(radians) * 15;

            ctx.beginPath();
            ctx.arc(sensorX, sensorY, 3, 0, 2 * Math.PI);
            ctx.fill();
        });
    }



}