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.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;

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

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


public class AutoRedFront 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,3000);

        TurnLeftTime(DRIVE_POWER,4000);


        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);
    }





}