package org.ftcTeam.utils;

import com.qualcomm.robotcore.hardware.DcMotor;

import org.ftcbootstrap.components.operations.motors.MotorToEncoder;
import org.ftcbootstrap.components.utils.MotorDirection;

/**
 * Created by dkim on 1/11/17.
 */

public class EncoderBasedAutonomousUtil {


    public static boolean pauseMotorEncoder(MotorToEncoder motorR, MotorToEncoder motorL, MotorToEncoder motorBR, MotorToEncoder motorBL) {

        motorR.stop();
        motorL.stop();
        motorBR.stop();
        motorBL.stop();
        return true;
    }

    public static boolean rotateMotor180(MotorToEncoder motorR, MotorToEncoder motorL, MotorToEncoder motorBR, MotorToEncoder motorBL) throws InterruptedException {
        //values
        int rotateMotor = 1140;
        boolean targetReached = false;

        targetReached =  motorR.runToTarget(.5, rotateMotor * (0), MotorDirection.MOTOR_FORWARD, DcMotor.RunMode.RUN_USING_ENCODER) &&
        motorL.runToTarget(.5, rotateMotor * (0), MotorDirection.MOTOR_FORWARD, DcMotor.RunMode.RUN_USING_ENCODER) &&
        motorBR.runToTarget(.5, rotateMotor * (0), MotorDirection.MOTOR_FORWARD, DcMotor.RunMode.RUN_USING_ENCODER) &&
        motorBL.runToTarget(.5, rotateMotor * (0), MotorDirection.MOTOR_FORWARD, DcMotor.RunMode.RUN_USING_ENCODER);

        if (targetReached) {
            return true;
        } else {
            return false;
        }
    }


    public static boolean strafLeft(MotorToEncoder motorR, MotorToEncoder motorL, MotorToEncoder motorBR, MotorToEncoder motorBL) throws InterruptedException{
        //values
        int rotateMotor = 1140;

        boolean isFRDone = motorR.runToTarget(1.0, rotateMotor * (1), MotorDirection.MOTOR_FORWARD, DcMotor.RunMode.RUN_USING_ENCODER);
        boolean isFLDone = motorL.runToTarget(1.0, rotateMotor * (1), MotorDirection.MOTOR_FORWARD, DcMotor.RunMode.RUN_USING_ENCODER);
        boolean isBRDone = motorBR.runToTarget(1.0, rotateMotor * (-1), MotorDirection.MOTOR_BACKWARD, DcMotor.RunMode.RUN_USING_ENCODER);
        boolean isBLDone = motorBL.runToTarget(1.0, rotateMotor * (-1), MotorDirection.MOTOR_BACKWARD, DcMotor.RunMode.RUN_USING_ENCODER);

        if (isFRDone && isFLDone && isBRDone && isBLDone) {
            return true;
        }
        else {
            return false;
        }
    }
}



@@ -1313,24 +1313,558 @@
 tReached) %7B%0A
+                    motorR.runToTarget(.5, rotateMotor * (0), MotorDirection.MOTOR_FORWARD, DcMotor.RunMode.STOP_AND_RESET_ENCODER);%0A                    motorL.runToTarget(.5, rotateMotor * (0), MotorDirection.MOTOR_FORWARD, DcMotor.RunMode.STOP_AND_RESET_ENCODER);%0A                    motorBR.runToTarget(.5, rotateMotor * (0), MotorDirection.MOTOR_FORWARD, DcMotor.RunMode.STOP_AND_RESET_ENCODER);%0A                    motorBL.runToTarget(.5, rotateMotor * (0), MotorDirection.MOTOR_FORWARD, DcMotor.RunMode.STOP_AND_RESET_ENCODER);%0A
             
@@ -1868,32 +1868,33 @@
    return true;%0A
+%0A
         %7D else %7B
@@ -2690,32 +2690,32 @@
 SING_ENCODER);%0A%0A
-
         if (isFR
@@ -2754,24 +2754,526 @@
 isBLDone) %7B%0A
+            motorR.runToTarget(.5, rotateMotor * (0), MotorDirection.MOTOR_FORWARD, DcMotor.RunMode.STOP_AND_RESET_ENCODER);%0A            motorL.runToTarget(.5, rotateMotor * (0), MotorDirection.MOTOR_FORWARD, DcMotor.RunMode.STOP_AND_RESET_ENCODER);%0A            motorBR.runToTarget(.5, rotateMotor * (0), MotorDirection.MOTOR_FORWARD, DcMotor.RunMode.STOP_AND_RESET_ENCODER);%0A            motorBL.runToTarget(.5, rotateMotor * (0), MotorDirection.MOTOR_FORWARD, DcMotor.RunMode.STOP_AND_RESET_ENCODER);%0A
             

Reset Encoders Added