public class Robot {

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

    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);
        this.pilot = new Pilot(this);

        // It might need more code
    }

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

    public void setPos(Vector2D pos) {
        this.pos = pos;
    }

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

    public DistanceSensor getRightSensor() {
        return rightSensor;
    }

    public DistanceSensor getFrontSensor() {
        return frontSensor;
    }

    public DistanceSensor getLeftSensor() {
        return leftSensor;
    }

    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 (this.pilot.setCourse())
            return;
        this.pos = this.pos.plus(this.vel.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 boolean finish() {
            double dx = this.direction.getX() * sensorRange + pos.getX();
            double dy = this.direction.getY() * sensorRange + pos.getY();
            return world.isThereFinish((int) dx, (int) dy);
        }

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

    public class Pilot {
        private DistanceSensor rs,fs,ls;
        private Robot robot;

        public Pilot(Robot r){
            this.robot = r;
            rs = r.getRightSensor();
            fs = r.getFrontSensor();
            ls = r.getLeftSensor();
        }

        public boolean setCourse() {
            if (fs.finish()) {
                return true;
            }

            if (fs.senseWall()) {
                turnLeft();
                return true;
            }

            if (rightHand != rs.senseWall()) {
                turnRight();
                return true;
            }

            return false;
        }

    }
}