import edu.cmu.ri.createlab.terk.robot.finch.Finch;
public class Assignment2Real {
    static Finch f = new Finch() ;
    static int minimumrequirment = 50; //the min requirment for light to be used
    static int left = (int) leftSensorValue();
    static int right = (int) rightSensorValue();

    public static void main (String args[]) throws InterruptedException  {
        System.out.println(leftSensorValue());
        System.out.println(rightSensorValue());

        run();



    }

    public static void run() throws InterruptedException {

        if (rightSensorValue() < minimumrequirment && leftSensorValue() < minimumrequirment && !f.isBeakUp() ) {
            noLightFound();

        }
        if (rightSensorValue() > minimumrequirment && leftSensorValue() > minimumrequirment && !f.isBeakUp()) {
            brightnessLevel();
            moveWithLight();

        }

        else if (f.isBeakUp()) {
            System.out.println("finch is beak up");
            f.quit();
        }

    }
    public static double rightSensorValue() {
        return f.getRightLightSensor();
    }
    //These 2 methods allow the light sensor to change values. Int wouldn't update so I chose method as it always gave me the changed values.
    public static double leftSensorValue() {
        return f.getLeftLightSensor();
    }

    public static void startUp() throws InterruptedException{
        if(leftSensorValue()<minimumrequirment && rightSensorValue()<minimumrequirment) {
            f.setLED(200,150,0); //change to yellow
            f.setWheelVelocities(50,50,3000); // moving at a slow speed for 4 seconds  correct in debug
        }
    }

    public static void moveWithLight() throws InterruptedException{
        if(leftSensorValue() > minimumrequirment &&  rightSensorValue() > minimumrequirment && leftSensorValue() > rightSensorValue() && !f.isBeakUp()) {
            f.setWheelVelocities(90,210);
            System.out.println("turning left");
        }
        if(leftSensorValue() > minimumrequirment &&  rightSensorValue() > minimumrequirment && rightSensorValue() > leftSensorValue() && !f.isBeakUp()) {
            f.setWheelVelocities(210,90);
            System.out.println("turning right");
        }
        if (leftSensorValue() < minimumrequirment &&  rightSensorValue() < minimumrequirment && !f.isBeakUp()){
            run();
            return; // add return as a failsafe method.
        }
        if (f.isBeakUp()) {
            run();
            return;
        }
        moveWithLight();
    }

    private static void noLightFound() throws InterruptedException {
        if(leftSensorValue()<minimumrequirment && rightSensorValue()<minimumrequirment && !f.isBeakUp()) {
            f.setLED(200,150,0); //change to yellow
            f.setWheelVelocities(50,50,3000); // moving at a slow speed for 4 seconds  correct in debug
        }
        // This keeps on moving finch 90 degrees at a time to find the light
        if (rightSensorValue() < minimumrequirment && leftSensorValue() < minimumrequirment && !f.isBeakUp()) {
            Thread.sleep(500);//stop 1/2 second
            f.setWheelVelocities(0,100,1350); //turn  Left 90 degrees
            if (rightSensorValue() > minimumrequirment && leftSensorValue() > minimumrequirment && !f.isBeakUp()) {
                System.out.println("Looking for Light");
                run();
                return;
            }
        }
        if (f.isBeakUp()) {
            run();
            return;
        }
        noLightFound(); //recursive method
    }

    public static void brightnessLevel() throws InterruptedException{ //making finch beak change Brightness
        if(leftSensorValue() > (minimumrequirment + 1) && leftSensorValue() <=99 && rightSensorValue() > (minimumrequirment +1) && rightSensorValue() <=99 && !f.isBeakUp() );{ //if finch sensors are between these variables, then the brightness changes correct with debug
            f.buzz(500, 2000);
            f.setLED(20,0,0);
        }
        if(leftSensorValue() >= 100 && leftSensorValue() <=170 && rightSensorValue() >= 100 && rightSensorValue() <=170 && !f.isBeakUp()) {
            f.buzz(500, 2000);
            f.setLED(100,0,0);
        }

        if(leftSensorValue() >= 171 && leftSensorValue() <=255 && rightSensorValue() >= 171 && rightSensorValue() <=255 && !f.isBeakUp()) {
            f.setLED(255,0,0);
        }

        if (f.isBeakUp()) {
            run();
            return;

        }
    }




}