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

import com.kauailabs.navx.frc.AHRS;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkLowLevel.MotorType;

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

public class Grimpeur extends SubsystemBase {
  /** Creates a new Acrocheur. */
  // moteur
  
  ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
  ShuffleboardLayout layout = Shuffleboard.getTab("dashboard")
  .getLayout("grimpeur", BuiltInLayouts.kList)
  .withSize(2,4)
  .withPosition(1,1);
  final CANSparkMax grimpeurd = new CANSparkMax(Constants.grimpeurd,MotorType.kBrushless);
  final CANSparkMax grimpeurg = new CANSparkMax(Constants.grimpeurg,MotorType.kBrushless); 
  // limit switch
  final DigitalInput limitdroite = new DigitalInput(Constants.limithaut);
  final DigitalInput limitgauche = new DigitalInput(Constants.limitbas);
  final DoubleSolenoid pistondroite= new DoubleSolenoid(PneumaticsModuleType.CTREPCM, Constants.pistondroiteouvre, Constants.pistondroiteouvre);
  final DoubleSolenoid pistondgauche= new DoubleSolenoid(PneumaticsModuleType.CTREPCM, Constants.pistondgaucheouvre, Constants.pistondroiteouvre);
  //fonction
  public Grimpeur() {
    layout.addBoolean("limitgrimpeurd", limitdroite::get);
    layout.addBoolean("limitgrimpeurg", limitdroite::get);
    layout.add("grimpeurencodeurd", encoderd());
    layout.add("grimpeurencodeurg", encoderg());
    layout.add("pitchgyrogrimpeur", getpitch());
  }
public void droit(double vitesse){
  grimpeurd.set(vitesse);
} 
public void gauche(double vitesse){
  grimpeurg.set(vitesse);
} 
public boolean droite(){
  return limitdroite.get();
}
public boolean gauche(){
  return limitgauche.get();
}
public void resetencodeurd(){
  grimpeurd.getEncoder().setPosition(0);
}
public void resetencodeurg(){
  grimpeurg.getEncoder().setPosition(0);
}
public double encoderd(){
 return grimpeurd.getEncoder().getPosition();
}
public double encoderg(){
  return grimpeurg.getEncoder().getPosition();
}
public AHRS gyroscope = new AHRS();
  public double getpitch(){
    return gyroscope.getPitch();
  }
  public boolean pistonouvre(){
    pistondroite.set(Value.kForward);
    pistondgauche.set(Value.kForward);
    return pistondroite.isFwdSolenoidDisabled();
  }
  public void pistonferme(){
    pistondroite.set(Value.kReverse);
    pistondgauche.set(Value.kReverse);
  
  }
  @Override
  public void periodic() {
    // This method will be called once per scheduler run
    if(droite()) {
      resetencodeurd();
    }
    if(gauche()) {
      resetencodeurg();
    }
  }
}