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 = "AutoBlueFront", group = "Autonomous")


    public class AutoBlueFront 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,2000);

            TurnLeftTime(DRIVE_POWER,900);

            DriveFowardTime(DRIVE_POWER,12000);


            //finish the objective and stop
            StopDriving();



        }

        //make "method" for functions

        double DRIVE_POWER = 1.0;

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

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

        public void DriveBackwards (double power) {
            motorLeft.setPower(-0.6);
            motorRight.setPower(-0.6);
        }
        public void DriveBackwards (double power, long time) throws InterruptedException
        {
            DriveBackwards(0.6);
            Thread.sleep(1000);
        }

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

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

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

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

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





    }