package frc.robot.subsystems;

import java.util.function.DoubleSupplier;

import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.motorcontrol.VictorSP;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.DrivetrainConstants;

public class Drivetrain extends SubsystemBase {
    
    //creates instance variables//
    private VictorSP leftFront;
    private VictorSP leftRear;
    private VictorSP rightFront;
    private VictorSP rightRear;

    private DifferentialDrive drive;

    public Drivetrain() {

        //creates instances of each motor//
        leftFront = new VictorSP(DrivetrainConstants.kLeftFrontPWM);
        leftRear = new VictorSP(DrivetrainConstants.kLeftRearPWM);
        rightFront = new VictorSP(DrivetrainConstants.kRightFrontPWM);
        rightRear = new VictorSP(DrivetrainConstants.kRightRearPWM);

        //makes the other motor in a gearbox follow its companion//
        leftFront.addFollower(leftRear);
        rightFront.addFollower(rightRear);
        rightFront.setInverted(true); //makes the right side of the drive base inverted//

        drive = new DifferentialDrive(leftFront, rightFront);    
    }
    
    //methods that take 2 doubles and convert them to motor commands//
    public Command driveArcade(DoubleSupplier xSpeed, DoubleSupplier zRotation) {
        return run(() -> {
            drive.arcadeDrive(xSpeed.getAsDouble(), zRotation.getAsDouble());
        });
    }

    public Command driveTank(DoubleSupplier leftSpeed, DoubleSupplier rightSpeed) {
        return run(() -> {
            drive.tankDrive(leftSpeed.getAsDouble(), rightSpeed.getAsDouble());
        });
    }
}