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


import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.PneumaticsModuleType;
import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

public class BrasTelescopique extends SubsystemBase {
  ShuffleboardLayout layout = Shuffleboard.getTab("teb")
  .getLayout("bras", BuiltInLayouts.kList)
  .withSize(2, 2);
  
  /** Creates a new BrasTelescopique. */
  public BrasTelescopique() {
    layout .addBoolean("photocell",this::photocell);
    layout .addDouble("encodeur",this::distance);
  }
  final CANSparkMax Winch = new CANSparkMax(Constants.BrasTelescopique,MotorType.kBrushless);
  private DigitalInput photocell = new DigitalInput(Constants.photocell);
  private DoubleSolenoid brakewinch = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,Constants.brakewinchf, Constants.brakewinchb);
  public void AvanceRecule(double vitesse) {
    Winch.set(vitesse);
  }
  public double distance() {
    return(Winch.getEncoder().getPosition());
  }
  public void Reset() {
    Winch.getEncoder().setPosition(0);
  }
  public boolean photocell(){
    return photocell.get(); 
  }
  public void fermer() {
    brakewinch.set(Value.kReverse);
    }
    public void ouvrir() {
      brakewinch.set(Value.kForward);
    }
  @Override
  public void periodic() {
    if(photocell())Reset();
  }
}