public class Robot {

    private Vector2D pos;
    private Vector2D vel;
    private MyWorld world;
    private DistanceSensor rightSensor, frontSensor, leftSensor;
    private boolean rightHand;


    // Constructor overload
    public Robot() {
        this(new Vector2D(), new Vector2D());
    }

    public Robot(Vector2D position, Vector2D velocity) {
        this.pos = position;
        this.vel = velocity;
        Vector2D dir = vel.getUnitary();
        rightSensor = new DistanceSensor(new Vector2D(dir.getY(), -dir.getX()), 1);
        frontSensor = new DistanceSensor(dir, 1);
        leftSensor = new DistanceSensor(new Vector2D(-dir.getY(), dir.getX()), 1);
    }

    public Robot(Vector2D position, Vector2D velocity, double sensorRange, MyWorld w) {
        this.pos = position;
        this.vel = velocity;
        this.world = w;
        Vector2D dir = vel.getUnitary();
        rightSensor = new DistanceSensor(new Vector2D(-dir.getY(), dir.getX()), sensorRange);
        frontSensor = new DistanceSensor(dir, sensorRange);
        leftSensor = new DistanceSensor(new Vector2D(dir.getY(), dir.getX()), sensorRange);
        // It might need more code
    }

    public Vector2D getPosition() {
        return this.pos;
    }

    public Vector2D getVelocity() {
        return this.vel;
    }

    public void turnLeft() {
        System.out.println("Turning left ...");
        this.vel = this.vel.rotate(90);
        leftSensor.turnLeft();
        frontSensor.turnLeft();
        rightSensor.turnLeft();
        this.rightHand = rightSensor.senseWall();
    }

    public void turnRight() {
        System.out.println("Turning right ...");
        this.vel = this.vel.rotate(-90);
        leftSensor.turnRight();
        frontSensor.turnRight();
        rightSensor.turnRight();
        this.rightHand = rightSensor.senseWall();
    }

    public void moveDelta_t(double delta_t) {
        if (frontSensor.senseWall()) {
            turnRight();
            return;
        }
        // This returns a new Vector2D
        this.pos = this.pos.plus(getVelocity().times(delta_t));
    }

    public String getDescription() {
        return pos.getDescription();
    }

    public String toString() {
        return pos.toString();
    }

    public class DistanceSensor {
        private Vector2D direction;
        private double sensorRange;

        public DistanceSensor (Vector2D dir, double sr) {
            this.direction = dir;
            this.sensorRange = sr;
        }

        public void turnLeft() {
            this.direction = direction.rotate(90);
        }

        public void turnRight() {
            this.direction = direction.rotate(-90);
        }

        public boolean senseWall() {
            double dx = this.direction.getX() * sensorRange + pos.getX();
            double dy = this.direction.getY() * sensorRange + pos.getY();

            return world.isThere_a_wall((int) dx, (int) dy);
        }

        public String toString() {
            return " "+senseWall();
        }
    }
}