import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.MotorPort;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;


public class CraneProject {


    public static void main(String[] args) throws InterruptedException {
        screen();
        down();
        clawclose();
        up();
        forward(10);
        down();
        clawopen();
        up();
        back();
    }

    public static void screen() throws InterruptedException{
        LCD.drawString("Press Any Button to", 0, 0);
        LCD.drawString("      Start", 0, 1);
        LCD.drawString("   The Program", 0, 2);
        Button.waitForAnyPress();
        LCD.clear();
        Thread.sleep(1000);
    }

    public static void forward(int distance) throws InterruptedException{
        MotorPort motor1 = MotorPort.A;
        UltrasonicSensor sonic= new UltrasonicSensor(SensorPort.S3);

        motor1.resetTachoCount();

        while(true){
            motor1.controlMotor(80, 1);                 //Setting motor speed and direction
            if(sonic.getDistance()< distance)           //When the crane goes close to the wall it stops
                break;
        }
        stop1();

    }

    public static void back() throws InterruptedException{
        MotorPort motor1 = MotorPort.A;
        motor1.controlMotor(80, 2);
        while(motor1.getTachoCount()>0);                //The Crane goes back to it's original position
        stop1();
    }

    public static void down() throws InterruptedException{
        TouchSensor touch= new TouchSensor(SensorPort.S4);
        MotorPort motor2=MotorPort.B;

        motor2.resetTachoCount();
        motor2.controlMotor(65, 1);
        while(!touch.isPressed())                       //When the touch sensor is pressed the downwards movement stops
        {
            try
            {
                Thread.sleep(1);
            } catch (InterruptedException e) {
                e.printStackTrace();
            }
        }
        stop2();
    }

    public static void up() throws InterruptedException{
        MotorPort motor2 = MotorPort.B;
        motor2.controlMotor(65, 2);
        while(motor2.getTachoCount()>0);                //After the crane grabs the box it lifts it back up
        stop2();
    }


    public static void clawclose() throws InterruptedException{
        MotorPort motor3=MotorPort.C;
        motor3.controlMotor(65, 2);
        {
            Thread.sleep(505);                          //Thread sleep is used for the cranes precise stopping
        }
        motor3.controlMotor(100, 3);
        {
            Thread.sleep(1000);
        }
        stop3();

    }



    public static void clawopen() throws InterruptedException{
        MotorPort motor3=MotorPort.C;
        motor3.controlMotor(65, 1);
        {
            Thread.sleep(520);
        }
        motor3.controlMotor(100, 3);
        {
            Thread.sleep(1000);
        }
        stop3();

    }


    //Codes for stopping of Motor 1, 2, and 3.
    public static void stop1 () throws InterruptedException{
        MotorPort motor1 = MotorPort.A;
        motor1.controlMotor(100, 3);
        Thread.sleep(1000);
    }
    public static void stop2 () throws InterruptedException{
        MotorPort motor1 = MotorPort.B;
        motor1.controlMotor(100, 3);
        Thread.sleep(1000);
    }
    public static void stop3 () throws InterruptedException{
        MotorPort motor1 = MotorPort.C;
        motor1.controlMotor(100, 3);
        Thread.sleep(1000);
    }

}