package org.firstinspires.ftc.teamcode;

// This `import` statements are used to pull in the FTC code into our program
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;

// Assign this teleop program a specific name so that we can choose it on the driver station
@TeleOp(name="PowerPlay Base", group="Linear Opmode")

public class PowerPlay_Base extends LinearOpMode {

    // This is a timer. It's effectively a stopwatch that we can use to check
    // how long the robot has been running. This is useful for defining movements
    // such as "Move forward for 1 second"
    private ElapsedTime runtime = new ElapsedTime();

    // These variables represent the physical motors/servos on our robot. In order to adjust
    // things like motor speed or servo position we have to use these variables. These
    // are set to `null` by default because we need to ask the Control Hub to actually find
    // the hardware before we can use it
    private DcMotor leftFront  = null;
    private DcMotor rightFront = null;
    private DcMotor leftRear   = null;
    private DcMotor rightRear  = null;

    private CRServo intake1    = null;
    private CRServo intake2    = null;

    // This function contains the core of our program. When the teleop is selected and started,
    // it automatically calls this function. This is where our gameplay logic will go
    @Override
    public void runOpMode() {
        // This adds data to our driver station dashboard. This is purely informational, it does
        // not have any impact on the robot
        telemetry.addData("Status", "Initialized");
        telemetry.update();

        // This is where we find the physical motors and actually save them into our
        // variables that are declared above. On the driver station we assign a name
        // to each physical port on the control hub + expansion hub. Then we use
        // those names in our program to find each motor/servo
        leftFront  = hardwareMap.get(DcMotor.class, "leftFront");
        rightFront = hardwareMap.get(DcMotor.class, "rightFront");
        leftRear   = hardwareMap.get(DcMotor.class, "leftRear");
        rightRear  = hardwareMap.get(DcMotor.class, "rightRear");

        intake1  = hardwareMap.get(CRServo.class, "intake1");
        intake2  = hardwareMap.get(CRServo.class, "intake2");

        // Here we set the direction of each of our motors. Because the motors are mirrored
        // on the right vs. the left of the robot, they're effectively backwards. This is
        // an easy way to account for that
        leftFront.setDirection(DcMotor.Direction.FORWARD);
        leftRear.setDirection(DcMotor.Direction.FORWARD);
        rightRear.setDirection(DcMotor.Direction.REVERSE);
        rightFront.setDirection(DcMotor.Direction.REVERSE);

        // Wait for the game to start (driver presses PLAY)
        waitForStart();

        // Reset the timer (stopwatch) because we only care about time since the game
        // actually starts
        runtime.reset();

        // This `while` loop runs until the end of the game. All of the code inside the loop
        // gets run over and over in order to control our robot. We do things such as
        // reading the controller input and setting motor speeds inside this loop because we
        // need to do those things constantly while we control our robot
        while (opModeIsActive()) {

            // Reads the controller inputs and stores the values into variables representing
            // how the robot will move
            double drive  = -gamepad1.left_stick_y;  // forwards and backwards movement
            double strafe =  gamepad1.left_stick_x;  // side to side movement
            double turn   =  gamepad1.right_stick_x; // rotation

            // Mecanum is weird. Because of the physics of how mecanum wheels actually move
            // the robot we have to do some weird math in order to get our robot to move how
            // we want it to. How this math works isn't very important. If you want more
            // information, ask Orion
            leftFront.setPower(Range.clip(  drive + strafe + turn, -1.0, 1.0));
            rightFront.setPower(Range.clip( drive - strafe - turn, -1.0, 1.0));
            leftRear.setPower(Range.clip(   drive - strafe + turn, -1.0, 1.0));
            rightRear.setPower(Range.clip(  drive + strafe - turn, -1.0, 1.0));

            // reads the controller inputs and stores the values into variables representing
            // how we want to control intake servos
            boolean intakeServoIn  = gamepad1.right_bumper;
            boolean intakeServoOut = gamepad1.left_bumper;

            // Adds the data for how our robot is moving to the driver station dashboard so
            // we can verify that controller input is being used properly
            telemetry.addData("Drive", drive);
            telemetry.addData("Strafe", strafe);
            telemetry.addData("Turn", turn);

            // This variable stores the value that we're going to use to control our intake servo.
            // We default it to 0 because we want the intake servos to remain still if
            // we're not pushing any buttons
            double intakePower = 0.0;

            // If the "intake in" button is pressed, set the intake servos to full speed forward
            if (intakeServoIn == true)
            {
                intakePower = 1;
            }
            // Otherwise, if the "intake in" button is pressed, set the intake servos to
            //  full speed backward
            // Note: Because we use "else if", this code will never run if the previous
            // condition (if (intakeServoIn == true)) was true. This means that if we push
            // the "intake in" button, pressing the "intake out" button as well does nothing
            else if (intakeServoOut == true)
            {
                intakePower = -1;
            }

            // Sets the power of both intake servers.
            intake1.setPower(intakePower);
            // Note: The value of `intakePower` gets multiplied by -1 so that intake2 turns in
            // the opposite direction of intake1
            intake2.setPower(intakePower * -1);

            // This is what actually sends all the telemetry data to the driver station
            telemetry.update();
        }
    }
}