// 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.SparkMax;
import com.revrobotics.spark.SparkLowLevel.MotorType;

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

public class Grimpeur extends SubsystemBase {
  ShuffleboardTab teb = Shuffleboard.getTab("teb");
  SparkMax grimpeur1 = new SparkMax(3, MotorType.kBrushless);
  SparkMax grimpeur2 = new SparkMax(12, MotorType.kBrushless);

  // SparkMaxConfig slaveConfig = new SparkMaxConfig();
  DigitalInput limit = new DigitalInput(0);
  private GenericEntry EncodeurGrimpeur = teb.add("Position haut grimpeur", 100).getEntry();

  // public void Grimper(double vitesse){
  // grimpeur1.configure(slaveConfig, ResetMode.kNoResetSafeParameters,
  // PersistMode.kPersistParameters);
  // grimpeur2.configure(slaveConfig.follow(grimpeur1),
  // ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
  // grimpeur1.set(vitesse);
  // }
  public void GrimperGauche(double vitesse) {
    grimpeur1.set(vitesse);
  }

  public void GrimperDroit(double vitesse) {
    grimpeur2.set(vitesse);
  }

  public double Position() {
    return grimpeur1.getEncoder().getPosition();
  }

  public void Reset() {
    grimpeur1.getEncoder().setPosition(0);
  }

  public boolean Limit() {
    return limit.get();
  }

  public double PositionFinal() {
    return EncodeurGrimpeur.getDouble(100);
  }

  /** Creates a new Grimpeur. */
  public Grimpeur() {
    teb.addDouble("encodeur grimpeur", this::Position);
    teb.addBoolean("limit grimpeur", this::Limit);
  }

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