package frc.robot.subsystems;

import java.util.function.DoubleSupplier;

import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
import com.ctre.phoenix6.controls.Follower;
import com.ctre.phoenix6.hardware.TalonFX;
import com.revrobotics.CANSparkFlex;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkLowLevel.MotorType;

import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.Talon;
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 MotorController leftFront;
    private MotorController leftRear;
    private MotorController rightFront;
    private MotorController rightRear;

    private DifferentialDrive drive;

    public Drivetrain() {
    /* VICTOR SP STYLE
        leftFront = new VictorSP(DrivetrainConstants.kLeftFrontID);
        leftRear = new VictorSP(DrivetrainConstants.kLeftRearID);
        rightFront = new VictorSP(DrivetrainConstants.kRightFrontID);
        rightRear = new VictorSP(DrivetrainConstants.kRightRearID);

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

    /* CAN SPARK MAX STYLE
        leftFront = new CANSparkMax(DrivetrainConstants.kLeftFrontID, MotorType.kBrushless);
        leftRear = new CANSparkMax(DrivetrainConstants.kLeftRearID, MotorType.kBrushless);
        rightFront = new CANSparkMax(DrivetrainConstants.kRightFrontID, MotorType.kBrushless);
        rightRear = new CANSparkMax(DrivetrainConstants.kRightRearID, MotorType.kBrushless);

        ((CANSparkMax)leftRear).follow((CANSparkMax)leftFront);
        ((CANSparkMax)rightRear).follow((CANSparkMax)rightFront);
        
        rightFront.setInverted(true);
    */

    /* CAN SPARK FLEX STYLE
        leftFront = new CANSparkFlex(DrivetrainConstants.kLeftFrontID, MotorType.kBrushless);
        leftRear = new CANSparkFlex(DrivetrainConstants.kLeftRearID, MotorType.kBrushless);
        rightFront = new CANSparkFlex(DrivetrainConstants.kRightFrontID, MotorType.kBrushless);
        rightRear = new CANSparkFlex(DrivetrainConstants.kRightRearID, MotorType.kBrushless);

        ((CANSparkMax)leftRear).follow((CANSparkMax)leftFront);
        ((CANSparkMax)rightRear).follow((CANSparkMax)rightFront);
        
        rightFront.setInverted(true);

    */

    /* KRAKEN X60/FALCON 500 STYLE
        leftFront = new TalonFX(DrivetrainConstants.kLeftFrontID);
        leftRear = new TalonFX(DrivetrainConstants.kLeftRearID);
        rightFront = new TalonFX(DrivetrainConstants.kRightFrontID);
        rightRear = new TalonFX(DrivetrainConstants.kRightRearID);

        ((TalonFX)leftRear).setControl(new Follower(DrivetrainConstants.kLeftFrontID, false));
        ((TalonFX)rightRear).setControl(new Follower(DrivetrainConstants.kRightFrontID, false));
        rightFront.setInverted(true);
    */

    /* TALON SRX STYLE 
        leftFront = new WPI_TalonSRX(DrivetrainConstants.kLeftFrontID);
        leftRear = new WPI_TalonSRX(DrivetrainConstants.kLeftRearID);
        rightFront = new WPI_TalonSRX(DrivetrainConstants.kRightFrontID);
        rightRear = new WPI_TalonSRX(DrivetrainConstants.kRightRearID);

        ((WPI_TalonSRX)leftRear).follow((WPI_TalonSRX)leftFront);
        ((WPI_TalonSRX)rightRear).follow((WPI_TalonSRX)rightFront);
        rightFront.setInverted(true);
    */

    /* VICTOR SPX STYLE 
        leftFront = new WPI_VictorSPX(DrivetrainConstants.kLeftFrontID);
        leftRear = new WPI_VictorSPX(DrivetrainConstants.kLeftRearID);
        rightFront = new WPI_VictorSPX(DrivetrainConstants.kRightFrontID);
        rightRear = new WPI_VictorSPX(DrivetrainConstants.kRightRearID);

        ((WPI_VictorSPX)leftRear).follow((WPI_VictorSPX)leftFront);
        ((WPI_VictorSPX)rightRear).follow((WPI_VictorSPX)rightFront);
        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());
        });
    }
}
