import lejos.hardware.Button;
import lejos.hardware.Sound;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.BaseRegulatedMotor;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;

public class RacingCar {

    public static void main(String[] args) {
        final int FORWARD_ROTATE_ANGLE = 720;
        final int TURN_ANGLE = 180;
        final int TURN_SPEED = 200;
        final int TURNS = 4;
        final int STALL_ERROR = 10;
        final int STALL_TIME = 2;


        BaseRegulatedMotor mLeft = new EV3LargeRegulatedMotor(MotorPort.A);
        BaseRegulatedMotor mRight = new EV3LargeRegulatedMotor(MotorPort.B);

        final int FORWARD_SPEED = (int) mLeft.getMaxSpeed();

        //mLeft.setStallThreshold(STALL_ERROR, STALL_TIME);
        //mRight.setStallThreshold(STALL_ERROR, STALL_TIME);

        mLeft.synchronizeWith(new BaseRegulatedMotor[] {mRight});


        for (int turn = 0; turn < TURNS; turn++) {
            LCD.drawString("Loop: " + turn, 0, 0);

            //Sends the robot forward
            Forward(mLeft, mRight, FORWARD_SPEED, FORWARD_ROTATE_ANGLE);

            //Wait for motors to stop rotating forward
            mLeft.waitComplete();

            //if (turn == 4)break;
            //Turns the robot at the corner
            Turn(mLeft, mRight, TURN_SPEED, TURN_ANGLE);

            //Wait for robot to stop turning
            mLeft.waitComplete();

        }

        //Saves data on screen
        Button.ENTER.waitForPressAndRelease();

        mLeft.close();
        mRight.close();

    }

    private static void Forward(BaseRegulatedMotor mLeft, BaseRegulatedMotor mRight, int motorSpeed, int rotateAngle) {
        //Creates and runs synchronised motors
        mLeft.startSynchronization();

        //Set the speed of the motors for going forward
        mLeft.setSpeed(motorSpeed);
        mRight.setSpeed(motorSpeed);

        //Rotates both motors in sync moving it forward
        mLeft.rotate(rotateAngle, false);
        mRight.rotate(rotateAngle, false);

        //Check if the motors have stalled
        /*while (mLeft.isStalled() || mRight.isStalled()) {

            //Stop the motors and play beep
            mLeft.stop();
            mRight.stop();

            Sound.playTone(100, 3, 100);
        }*/
        mLeft.endSynchronization();
        LCD.drawString("Moving forward...", 1, 0);
    }

    private static void Turn(BaseRegulatedMotor mLeft, BaseRegulatedMotor mRight, int motorSpeed, int rotateAngle) {
        //Creates and synchronises the rotating of the robot
        mLeft.startSynchronization();

        //Sets the motor speeds for turning
        mLeft.setSpeed(motorSpeed);
        mRight.setSpeed(motorSpeed);

        //Rotates the motors in opposite direction to turn 90 degrees
        mLeft.rotate(rotateAngle, false);
        mRight.rotate(-rotateAngle, false);

        //Check if the motors have stalled
        /*while (mLeft.isStalled() || mRight.isStalled()) {

            //Stop the motors and play beep
            mLeft.stop();
            mRight.stop();

            Sound.playTone(100, 3, 100);
        }*/
        mLeft.endSynchronization();
        LCD.drawString("Turning...", 1, 0);

    }

}