// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.subsystem;

import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkLowLevel.MotorType;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.MutableMeasure;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.units.Voltage;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Constants;
import static edu.wpi.first.units.Units.Volts;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.MutableMeasure.mutable;
public class Lanceur extends SubsystemBase {
  /** Creates a new Lanceur. */
  ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
   private GenericEntry vitesse =
      dashboard.add("vitesse", 0.8)
      .withSize(1, 1)
      .withPosition(2,1)
         .getEntry();
         private GenericEntry vitesseamp =
            dashboard.add("vitesseamp", 0.1)
            .withSize(1, 1)
            .withPosition(2,3)
              .getEntry();
  public Lanceur() {
    
  }
  final CANSparkMax lanceur1 = new CANSparkMax(Constants.lanceur, MotorType.kBrushless);
  final CANSparkMax lanceur2 = new CANSparkMax(Constants.lancer2, MotorType.kBrushless);
  final CANSparkMax lanceur3 = new CANSparkMax(Constants.lancer3, MotorType.kBrushless);
  final CANSparkMax lanceur4 = new CANSparkMax(Constants.lancer4, MotorType.kBrushless);
  private final MutableMeasure<Voltage> appliedVoltage = mutable(Volts.of(0));
  // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation.
  private final MutableMeasure<Angle> angle = mutable(Rotations.of(0));
  // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation.
  private final MutableMeasure<Velocity<Angle>> velocity = mutable(RotationsPerSecond.of(0));
  public void pidspeaker(){
    lanceur1.getPIDController().setP(0.5);
    lanceur1.getPIDController().setI(0.5);
    lanceur1.getPIDController().setD(0.5);
    lanceur2.getPIDController().setP(0.5);
    lanceur2.getPIDController().setI(0.5);
    lanceur2.getPIDController().setD(0.5);
    lanceur3.getPIDController().setP(0.5);
    lanceur3.getPIDController().setI(0.5);
    lanceur3.getPIDController().setD(0.5);
    lanceur4.getPIDController().setP(0.5);
    lanceur4.getPIDController().setI(0.5);
    lanceur4.getPIDController().setD(0.5);
  }
   private final SysIdRoutine sysIdRoutine =
      new SysIdRoutine(
          // Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage.
          new SysIdRoutine.Config(),
          new SysIdRoutine.Mechanism(
              // Tell SysId how to plumb the driving voltage to the motor(s).
              (Measure<Voltage> volts) -> {
                lanceur4.setVoltage(volts.in(Volts));
              },
              // Tell SysId how to record a frame of data for each motor on the mechanism being
              // characterized.
              log -> {
                // Record a frame for the shooter motor.
                log.motor("shooter-wheel")
                    .voltage(
                        appliedVoltage.mut_replace(
                            lanceur4.getAppliedOutput() * lanceur4.getBusVoltage(), Volts))
                    .angularPosition(angle.mut_replace(lanceur4.getEncoder().getPosition(), Rotations))
                    .angularVelocity(
                        velocity.mut_replace(lanceur4.getEncoder().getVelocity(), RotationsPerSecond));
              },
              // Tell SysId to make generated commands require this subsystem, suffix test state in
              // WPILog with this subsystem's name ("shooter")
              this));
  public void lancer(double vitesse){
    lanceur1.set(-vitesse);
    lanceur2.set(-vitesse);
    lanceur3.set(vitesse);
    lanceur4.set(-vitesse);
  }
  public void lancerspeaker(){
    lancer(vitesse.getDouble(0.8));
  }
  public void lanceramp(){
    lancer(vitesseamp.getDouble(0.1));
  }
  public double vitesse(double vitesse){
    return lanceur3.getEncoder().getVelocity();
  }
    public void lanceur(){
   
  }
  public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
    return sysIdRoutine.quasistatic(direction);
  }

  /**
   * Returns a command that will execute a dynamic test in the given direction.
   *
   * @param direction The direction (forward or reverse) to run the test in
   */
  public Command sysIdDynamic(SysIdRoutine.Direction direction) {
    return sysIdRoutine.dynamic(direction);
  }
  @Override
  public void periodic() {}
    // This method will be called once per scheduler run
  
}