import  utmech.mechanompu.io.*;

public class WheelPosTest {
    private static long lwheel=0, rwheel=0;
    private static final int max_error= 50;

    private static boolean isarrived() {
        if(lwheel - WheelPos.l() > max_error)
            return false;
        if(lwheel - WheelPos.l() < -max_error)
            return false;
        if(rwheel - WheelPos.r() > max_error)
            return false;
        if(rwheel - WheelPos.r() < -max_error)
            return false;
        return true;
    }
    
    private static void fwd(long dist) {
        lwheel += dist;
        rwheel += dist;
        WheelPos.l(lwheel);
        WheelPos.r(rwheel);
        while(true) {
            if(isarrived())
                break;
            Thread.yield();
        }
    }

    private static void rot(long deg) {
        long dist=deg*480/360;
        lwheel -= dist;
        rwheel += dist;
        WheelPos.l(lwheel);
        WheelPos.r(rwheel);
        while(true) {
            if(isarrived())
                break;
            Thread.yield();
        }
    }
    
    public static void main(String[] args) {
        WheelPos.init();
        System.out.println();
        System.out.println();
        System.out.println("Pos. Cont. Test");
        System.out.print("Fwd 500");
        fwd(500);
        System.out.print("\nRot -90");
        rot(-90);
        System.out.print("\nFwd 300");
        fwd(300);
    }
}