package happywallrobot;

public class HappyWallRobot extends VacuumRobotControl implements Runnable{

    private final int STILL = 0;
    private final int FORWARD = 1;
    private final int SPINNING = 2;
    private final int WAITFORSTILL = 3;
    private int state = 0;

    // -------------------------------------------------------------------------
    public HappyWallRobot() {
    }

    // -------------------------------------------------------------------------
    // HERE IS THE THREAD CLOCKED STATE MACHINE!
    public void run() {
        long SLEEPYTIME = 100;
        System.out.println("Robot Control Thread started with " + 1000.0 / SLEEPYTIME + "Hz");
        while (true) {
            long startTime = System.currentTimeMillis();

            // --------------------------------------------------------------
            // call abstract method: run state machine!
            runStateMachine();
            // --------------------------------------------------------------

            // timing
            long currentTime = System.currentTimeMillis();
            long pauseTime = Math.max(1, SLEEPYTIME - (currentTime - startTime));
            try {
                Thread.sleep(pauseTime);
            } catch (InterruptedException ex) {
            }
        }
    }

    // -------------------------------------------------------------------------
    // HERE IS THE THREAD CLOCKED STATE MACHINE!
    private void runStateMachine() {
        long startTime = System.currentTimeMillis();
        switch (state) {
            case STILL:
                if (detectWall()) {
                    endSpinTime = startTime + 1000 + (long) (Math.random() * 1500);
                    setMotorSpeedLR(500, -500);
                    state = SPINNING;
                } else {
                    setMotorSpeedLR(500, 500);
                    state = FORWARD;
                }
                break;
            case FORWARD:
                if (detectWall()) {
                    endSpinTime = startTime + 1000 + (long) (Math.random() * 1500);
                    setMotorSpeedLR(800, -800);
                    state = SPINNING;
                }
                break;
            case SPINNING:
                if (endSpinTime <= startTime) {
                    setMotorSpeedLR(0, 0);
                    state = WAITFORSTILL;
                }
                break;
            case WAITFORSTILL:
                if (!detectSpinning()) {
                    state = STILL;
                }
                break;
        }
    }

    private boolean detectWall() {
        double[] laserDist = mc.laser.readPolar(false);
        double minDist = Double.POSITIVE_INFINITY;
        for (int i = 270 - 40; i < 270 + 40; i++) {
            if (laserDist[i] < minDist) {
                minDist = laserDist[i];
            }
        }
        boolean wall = minDist < 800;
        return (wall);
    }

    private boolean detectSpinning() {
        double[] lr = mc.robot.getVelocityLR();
        double[] linAng = mc.robot.velocityMotorLRToVelocityLinAngRad(lr[0], lr[1]);
        boolean spinning = linAng[1] != 0;
        return (spinning);
    }
}
