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

import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.bras.BrasTelescopique;

public class Bougerbras extends CommandBase {
  /** Creates a new bougerbras. */
  double distance;
  BrasTelescopique brasTelescopique;
  private GenericEntry forcebrasdescendre;
  private GenericEntry forcebrasmonter;
  
  public Bougerbras(BrasTelescopique brasTelescopique,double distance,GenericEntry forcebrasdescendre, GenericEntry forcebrasmonter) {
    this.brasTelescopique = brasTelescopique;
    this.distance = distance;
    this.forcebrasdescendre = forcebrasdescendre;
    this.forcebrasmonter = forcebrasmonter;
    addRequirements(brasTelescopique);
    // Use addRequirements() here to declare subsystem dependencies.
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
    
  }

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    if(brasTelescopique.distance()>distance+0.5 ) {
      brasTelescopique.AvanceRecule(forcebrasdescendre.getDouble(-0.35));
      
    }
    else if(brasTelescopique.distance()<distance-0.5) {
      brasTelescopique.AvanceRecule(forcebrasmonter.getDouble(0.35));
    }
    else {
      brasTelescopique.AvanceRecule(0);
    }
  }

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {
    brasTelescopique.AvanceRecule(0);
  }

  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    return brasTelescopique.distance()>distance-0.5 && brasTelescopique.distance()<distance+0.5;
  

  }
}