package frc.robot.subsystems;

import java.util.function.DoubleSupplier;

import com.ctre.phoenix.motorcontrol.can.VictorSPX;
import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;

import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

public class Drivetrain extends SubsystemBase {
    private WPI_VictorSPX frontLeft;
    private WPI_VictorSPX frontRight;
    private WPI_VictorSPX rearLeft;
    private WPI_VictorSPX rearRight;

    private DifferentialDrive dt;

    public Drivetrain() {
        frontLeft = new WPI_VictorSPX(4);
        frontRight = new WPI_VictorSPX(2);
        rearLeft = new WPI_VictorSPX(3);
        rearRight = new WPI_VictorSPX(1);

        frontLeft.setInverted(true);
        rearLeft.setInverted(true);

        rearLeft.follow(frontLeft);
        rearRight.follow(frontRight);

        dt = new DifferentialDrive(frontLeft, frontRight);
    }

    public Command arcadeDrive(DoubleSupplier forwardBackward, DoubleSupplier leftRight) {
        return run(() -> {
            dt.arcadeDrive(forwardBackward.getAsDouble(), leftRight.getAsDouble());
        });
    }

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