// 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 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;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkLowLevel.MotorType;
public class Elevateur extends SubsystemBase {
  ShuffleboardTab teb = Shuffleboard.getTab("teb");
  /** Creates a new Elevateur. */
    private GenericEntry encodeurelevateurL2bas =
    teb.add("encodeurelevateurL2bas", -1).getEntry();
    private GenericEntry encodeurelevateurL2haut =
    teb.add("encodeurelevateurL2haut", -0.9).getEntry();
    private GenericEntry encodeurelevateurL3bas =
    teb.add("encodeurelevateurL3bas", -1.9).getEntry();
    private GenericEntry encodeurelevateurL3haut =
    teb.add("encodeurelevateurL3haut", -2.11).getEntry();
    private GenericEntry encodeurelevateurL4bas =
    teb.add("encodeurelevateurL4bas", -6.6).getEntry();
    private GenericEntry encodeurelevateurL4haut =
    teb.add("encodeurelevateurL4haut", -6.2).getEntry();
    private GenericEntry encodeurelevateurstationbas =
    teb.add("encodeurelevateursationbas", -0.5).getEntry();
    private GenericEntry encodeurelevateurstationhaut =
    teb.add("encodeurelevateursationhaut", -0.4).getEntry();
       
  public Elevateur() {
    teb.addDouble("encodeur elevateur",this::position);
    teb.addBoolean("limit elevateur", this::limit2);
  }
  final SparkMax  monte = new SparkMax(22, MotorType.kBrushless);
  final DigitalInput limit2 = new DigitalInput(0);
  
  public double position(){ 
    return monte.getEncoder().getPosition();
  }

  public void vitesse(double vitesse){
    if (limit2()) {
      if (vitesse > 0) {
        monte.set(0);
      }
      else{
        monte.set(vitesse);
      }
    }
    else{
      monte.set(vitesse);
    }
  }
  public boolean limit2(){
    return limit2.get();
  } 
  public void reset(){
    monte.getEncoder().setPosition(0);
  }
  public double encodeurelevateurL2bas(){
    return encodeurelevateurL2bas.getDouble(-1);
  }
  public double encodeurelevateurL2haut(){
    return encodeurelevateurL2haut.getDouble(-0.9);
  }
  public double encodeurelevateurL3bas(){
    return encodeurelevateurL3bas.getDouble(-1.9);
  }
  public double encodeurelevateurL3haut(){
    return encodeurelevateurL3haut.getDouble(-2.11);
  }
  public double encodeurelevateurL4bas(){
    return encodeurelevateurL4bas.getDouble(-6.6);
  }
  public double encodeurelevateurL4haut(){
    return encodeurelevateurL4haut.getDouble(-6.2);
  }
  public double encodeurelevateurstationbas(){
    return encodeurelevateurstationbas.getDouble(-0.5);
  }
  public double encodeurelevateurstationhaut(){
    return encodeurelevateurstationhaut.getDouble(-0.4);
  }
  
  @Override
  public void periodic() {
    // This method will be called once per scheduler run
  }
}