// 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.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
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 Pince extends SubsystemBase {
  /** Creates a new Pince. */
  ShuffleboardTab teb = Shuffleboard.getTab("teb");
  public Pince() {
    teb.addBoolean("limit pince",this::position);
    teb.addDouble("encodeur pince", this::encodeurpivot);
    teb.addDouble("amperage corail", this::emperagecoral);
    teb.addDouble("amperage algue", this::emperagealgue);
  }
  final SparkMax coral = new SparkMax(20, MotorType.kBrushless);
  final SparkMax pivoti = new SparkMax(14, MotorType.kBrushless);
  final SparkMax algue1 = new SparkMax(16, MotorType.kBrushless);
  final SparkMax algue2 = new SparkMax(19, MotorType.kBrushless);
  final DigitalInput limit6 = new DigitalInput(9);
  
 
public void aspirecoral(double vitesse){
  coral.set(vitesse);
}
public void pivote(double vitesse){
  if (position()) {
    if (vitesse < 0) {
      pivoti.set(0);
    }
    else{
      pivoti.set(vitesse);
    }
  }
  else{
    pivoti.set(vitesse);
  }
}
public void aspirealgue(double vitesse){
  algue2.set(-vitesse);
  algue1.set(-vitesse);
}
public double encodeurpivot(){
  return pivoti.getEncoder().getPosition();
}
public boolean position(){
 return limit6.get();
}
public void reset(){
  pivoti.getEncoder().setPosition(0);
}
public double emperagecoral(){
  return coral.getOutputCurrent();
}
public double emperagealgue(){
  return algue1.getOutputCurrent();
}
public boolean x = false;

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