// 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.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
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.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.SubsystemBase;


public class Lanceur extends SubsystemBase {
  /** Creates a new Lanceur. */
  public Lanceur() {
   dashboard.addBoolean("limitswitch1", this::limitswitch1)
   .withSize(0,0).withPosition(1,2);
   dashboard.addBoolean("limitswitch2", this::limitswitch2)
   .withSize(0,0).withPosition(0,3);
   dashboard.addDouble("rottourel", this::distancetourel)
    .withSize(0,0).withPosition(1, 0);
}
  ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
  
  final WPI_TalonSRX lanceur1 = new WPI_TalonSRX(0);
  final WPI_TalonSRX lanceur2 = new WPI_TalonSRX(1);
  final CANSparkMax tourelle = new CANSparkMax(2, MotorType.kBrushless);
  final DigitalInput limitswitch1 = new DigitalInput(0);
  final DigitalInput limitswitch2 = new DigitalInput(1);
  private GenericEntry vitesse =
        dashboard.add("vitesselanceur", 0.2) 
        .withSize(0,0)
        .withPosition(0, 2)
        .getEntry();
        
  public void encodeur(double distance){
    lanceur1.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
    lanceur2.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
    lanceur1.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 1);
  }
  public void touchagedelimitswitch1(double distance){
    tourelle.getEncoder().setPosition(distance);
  }
  public void touchagedelimitswitch2(double distance){
    tourelle.getEncoder().setPosition(distance);
  }
  public void masterslave(){
    lanceur2.follow(lanceur1);
    lanceur2.setInverted(true);
  }
  public void lance(double vitesse){
    lanceur1.set(vitesse);
  }
  public void lance(){  
    lance(vitesse.getDouble(0.2));
  }
  public void tourelRotation(double x, double y, double rotation){
    tourelle.set(rotation);
  }

  public double vitessetourel(){
   return (tourelle.getEncoder().getVelocity()); 
  }
  public double distancetourel(){
    return (tourelle.getEncoder().getPosition());
  }
  public void PIDlanceur(double p, double i, double d) {
   lanceur1.config_kP(0, p);
   lanceur1.config_kI(0, i);
   lanceur1.config_kD(0, d);
  }
  public boolean limitswitch1(){
    return limitswitch1.get();
  }
   public boolean limitswitch2(){
    return limitswitch2.get();
  } 
  @Override
  public void periodic() {
    // This method will be called oncehttps://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json per scheduler run
  }
}