import lejos.nxt.*;
import lejos.robotics.navigation.*;

class Rally {
    public static void main(String[] args) {
        DifferentialPilot bil = new DifferentialPilot(8.0f, 11.5f, Motor.C, Motor.A, true);
        LightSensor lightLeft = new LightSensor(SensorPort.S3);
        LightSensor lightRight = new LightSensor(SensorPort.S2);
        int lightThreshold = 38;
        double startSving = 30;
        double svingSpeed = 15;
        double sving = startSving;
        double maxSpeed = 20;
        boolean right = false;
        boolean left = false;
        boolean backOn = false;
        while (true) {
            if ((lightLeft.readValue() < lightThreshold) && (lightRight.readValue() < lightThreshold)){
                bil.forward();
            }
            else if(lightLeft.readValue() < lightThreshold) {
                while(!right){
//                  bil.setTravelSpeed(svingSpeed);
                    bil.steer(sving);
                    while(!(lightLeft.readValue() < lightThreshold)) {
                        sving += 0.25;
                        System.out.println("Inkrementerer: " + sving);
                        if (backOn) {
                            right = true;
                        }
                    }if (!(lightLeft.readValue() < lightThreshold)) {
                        backOn = true;
                    }
/*
                    if(lightRight.readValue() < lightThreshold) {
                        System.out.println("RIGHT: " + sving);
                        bil.steer(-sving/4);
                        right = true;
    //                  bil.forward();
                    }
                    */
                }
/*          }else if(lightRight.readValue() < lightThreshold) {
                while(!left){
//                  bil.setTravelSpeed(svingSpeed);
                    bil.steer(-sving);
                    if(!(lightRight.readValue() < lightThreshold)) {
                        sving += 0.25;
                        System.out.println("Inkrementerer: " + sving);
                    }
                    if(lightLeft.readValue() < lightThreshold) {
                        System.out.println("LEFT: " + sving);
                        bil.steer(sving/4);
                        left = true;
//                      bil.forward();
                    }
                }
                */
            }else{
                bil.forward();
                bil.setTravelSpeed(maxSpeed);
                sving = startSving;
                right = false;
                left = false;
                backOn = false;
            }

        }
    }
}