package org.firstinspires.ftc.teamcode;


import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;

/**
 *Autonomous program for Swerve Robotics tutorial
 */

@Autonomous(name = "AutoBlueBack", group = "Autonomous")


public class AutoBlueBack extends LinearOpMode {
    //declare motors
    DcMotor motorLeft = null;
    DcMotor motorRight = null;

    DcMotor armLeft = null;
    DcMotor armRight = null;

    DcMotor pullLeft = null;
    DcMotor pullRight = null;

    //declare servo
    Servo collector = null;


    @Override
    public void runOpMode() throws InterruptedException {
        /* initialize our hardware variables. Note that the strings used here as parameters
         * to 'get' must correspond to the name you assigned during the robot configuration
         * step you did in the FTC Robot Controller app on the phone
         */

        //initialize motors
        motorLeft = hardwareMap.dcMotor.get("motorLeft");
        motorRight = hardwareMap.dcMotor.get("motorRight");

        motorLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
        motorRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

        motorLeft.setDirection(DcMotor.Direction.REVERSE);
        motorRight.setDirection(DcMotor.Direction.FORWARD);


        //wait for the game to start
        waitForStart();

        DriveBackwards(DRIVE_POWER, 500);

        TurnLeftTime(DRIVE_POWER, 2000);

        DriveFowardTime(DRIVE_POWER, 2000);

        TurnLeftTime(DRIVE_POWER, 1000);


        DriveFowardTime(DRIVE_POWER, 3000);


        //finish the objective and stop
        StopDriving();


    }

    //make "method" for functions

    double DRIVE_POWER = 1.0;

    public void DriveForward(double power) {
        motorLeft.setPower(0.4);
        motorRight.setPower(0.4);
    }

    public void DriveFowardTime(double power, long time) throws InterruptedException {
        DriveForward(0.4);
        Thread.sleep(2000);
    }

    public void DriveBackwards(double power) {
        motorLeft.setPower(-0.4);
        motorRight.setPower(-0.4);
    }

    public void DriveBackwards(double power, long time) throws InterruptedException {
        DriveBackwards(0.4);
        Thread.sleep(3000);
    }

    public void StopDriving() {
        DriveForward(0);
    }

    public void TurnLeft(double power) {
        motorLeft.setPower(-0.4);
        motorRight.setPower(0.4);
    }

    public void TurnLeftTime(double power, long time) throws InterruptedException {
        TurnLeft(0.4);
        Thread.sleep(2000);
    }

    public void TurnRight(double power) {
        TurnLeft(-0.4);
    }

    public void TurnRightTime(double power, long time) throws InterruptedException {
        TurnRight(0.4);
        Thread.sleep(2000);
    }


}