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.HardwareMap;
import com.qualcomm.robotcore.util.ElapsedTime;

import org.firstinspires.ftc.robotcontroller.external.samples.HardwarePushbot;
import org.firstinspires.ftc.robotcore.external.Telemetry;


public class Drive {
    /* Declare OpMode members. */
    private ElapsedTime runtime = new ElapsedTime();

    static final double COUNTS_PER_MOTOR_REV = 1440;    // eg: TETRIX Motor Encoder
    static final double DRIVE_GEAR_REDUCTION = 2.0;     // This is < 1.0 if geared UP
    static final double WHEEL_DIAMETER_INCHES = 4.0;     // Fornew figuring circumference
    static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
            (WHEEL_DIAMETER_INCHES * 3.1415);

    public DcMotor BDL;
    public DcMotor BDR;
    public DcMotor FDR;
    public DcMotor FDL;
    LinearOpMode opMode;

    public Drive(LinearOpMode ahwMap) {
        this.opMode = ahwMap;
        BDL = ahwMap.hardwareMap.dcMotor.get("BDL");
        BDR = ahwMap.hardwareMap.dcMotor.get("BDR");
        FDL = ahwMap.hardwareMap.dcMotor.get("FDL");
        FDR = ahwMap.hardwareMap.dcMotor.get("FDR");

        BDR.setDirection(DcMotor.Direction.REVERSE);
        BDL.setDirection(DcMotor.Direction.REVERSE);

        BDL.setPower(0);
        BDR.setPower(0);
        FDL.setPower(0);
        FDR.setPower(0);

        // Set all otors to run without encoders.
        // May want to use RUN_USING_ENCODERS if encoders are installed.
    }

    public void encoderDrive(double power,
                             double distance) {
        FDR.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        FDL.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        BDR.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        BDL.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        int newLeftTarget;
        int newRightTarget;

        newLeftTarget = FDR.getCurrentPosition() + (int)(distance * COUNTS_PER_INCH);
        newRightTarget = FDL.getCurrentPosition() + (int)(distance * COUNTS_PER_INCH);
        FDL.setTargetPosition(newLeftTarget);
        FDR.setTargetPosition(newRightTarget);
        BDL.setTargetPosition(newLeftTarget);
        BDR.setTargetPosition(newRightTarget);

        FDL.setPower(power);
        BDL.setPower(-power);
        FDR.setPower(power);
        BDR.setPower(power);

        //display telemetry data while turning
        while (opMode.opModeIsActive() && (FDL.isBusy() && FDR.isBusy())) {
            // Display it for the driver.
            opMode.telemetry.addData("Path1",  "Running to %7d :%7d", newLeftTarget,  newRightTarget);
            opMode.telemetry.addData("Path2",  "Running at %7d :%7d", FDL.getCurrentPosition(), FDR.getCurrentPosition());
            opMode.telemetry.update();
        }

        //stop after done
        FDL.setPower(0);
        BDL.setPower(0);
        FDR.setPower(0);
        BDR.setPower(0);

        FDR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
        FDL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
        BDR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
        BDL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    }
}