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 {
    private VictorSP leftFront;
    private VictorSP leftRear;
    private VictorSP rightFront;
    private VictorSP rightRear;

    private DifferentialDrive drive;

    public Drivetrain() {
        leftFront = new VictorSP(DrivetrainConstants.kLeftFrontPWM);
        leftRear = new VictorSP(DrivetrainConstants.kLeftRearPWM);
        rightFront = new VictorSP(DrivetrainConstants.kRightFrontPWM);
        rightRear = new VictorSP(DrivetrainConstants.kRightRearPWM);

        leftFront.addFollower(leftRear);
        rightFront.addFollower(rightRear);
        rightFront.setInverted(true);

        drive = new DifferentialDrive(leftFront, rightFront);
    }

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