// 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.subsystems;

import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkLowLevel.MotorType;

import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

public class Balayeuse extends SubsystemBase {
  ShuffleboardTab teb = Shuffleboard.getTab("teb");
  SparkFlex Balaye1 = new SparkFlex(2, MotorType.kBrushless);
  SparkFlex Balaye2 = new SparkFlex(20, MotorType.kBrushless);
  SparkMax Pivot = new SparkMax(8, MotorType.kBrushless);
  DigitalInput limit = new DigitalInput(9);
  private GenericEntry EncodeurBalayeuse =
  teb.add("Position bas balayeuse", 10).getEntry();
  private GenericEntry AmpBaleyeuse =
  teb.add("Ampérage Baleyeuse", 40).getEntry();
  public void Balayer(double vitesse){
    Balaye1.set(vitesse);
    Balaye2.set(vitesse);
  }
  public void Pivoter(double vitesse){
    Pivot.set(vitesse);
  }
  public double Distance(){
    return Pivot.getEncoder().getPosition();
  } 
  public void Reset(){
    Pivot.getEncoder().setPosition(0);
  }
  public boolean GetLimiSwtich(){
    return !limit.get();
  }
  public double Amp(){
    return Balaye2.getOutputCurrent();
  }
  public double AmpMax(){
    return AmpBaleyeuse.getDouble(40);
  }
  public void Temps(){
    Timer timer = new Timer();
    timer.start();
  }
  public double EncodeurBalayeuse(){
    return EncodeurBalayeuse.getDouble(10);
  }
  /** Creates a new Balayeuse. */
  public Balayeuse() {
    teb.addBoolean("limit balayeuse", this::GetLimiSwtich);
    teb.addDouble("encodeur balayeuse", this::Distance);
  }

  @Override
  public void periodic() {
    // This method will be called once per scheduler run
  }
}