package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.util.Range;

@TeleOp (name = "Template")
public class Template extends LinearOpMode {

    DcMotor frontRight, frontLeft, rearRight, rearLeft;

    @Override
    public void runOpMode() {

        //Hardware Map
        frontRight = hardwareMap.get(DcMotor.class, "fR");
        frontLeft = hardwareMap.get(DcMotor.class, "fL");
        rearRight = hardwareMap.get(DcMotor.class, "rR");
        rearLeft = hardwareMap.get(DcMotor.class, "rL");

        rearLeft.setDirection(DcMotorSimple.Direction.REVERSE);
        frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);

        //Motor Powers
        double frPower, flPower, rrPower, rlPower;
        
        waitForStart();

        while (opModeIsActive()) {
            
            //Mecanum Drive
            frPower = -gamepad1.left_stick_y - gamepad1.right_stick_x - gamepad1.left_stick_x;
            flPower = -gamepad1.left_stick_y + gamepad1.right_stick_x + gamepad1.left_stick_x;
            rrPower = -gamepad1.left_stick_y - gamepad1.right_stick_x + gamepad1.left_stick_x;
            rlPower = -gamepad1.left_stick_y + gamepad1.right_stick_x - gamepad1.left_stick_x;

            frPower = Range.clip(frPower, -1, 1);
            flPower = Range.clip(flPower, -1, 1);
            rrPower = Range.clip(rrPower, -1, 1);
            rlPower = Range.clip(rlPower, -1, 1);

            frontLeft.setPower(flPower);
            frontRight.setPower(frPower);
            rearLeft.setPower(rlPower);
            rearRight.setPower(rrPower);

        }

    }

}