// 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.wpilibj2.command.CommandBase;
import frc.robot.subsystems.bras.BrasTelescopique;
import frc.robot.subsystems.bras.Pivot;

public class PivoteBrasBas extends CommandBase {
  private BrasTelescopique brasTelescopique;
  private Pivot pivot;
  /** Creates a new PivoteBrasBas. */
  public PivoteBrasBas(BrasTelescopique brasTelescopique, Pivot pivot) {
    this.brasTelescopique = brasTelescopique;
    this.pivot = pivot;
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(brasTelescopique);
    addRequirements(pivot);
  }

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

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    if(brasTelescopique.distance()>-13.5){
      brasTelescopique.AvanceRecule(-0.2);
      brasTelescopique.fermer();
    }
  else if(brasTelescopique.distance()<-15.5) {
    brasTelescopique.AvanceRecule(0.2);
    }
    else {
      brasTelescopique.AvanceRecule(0);
      brasTelescopique.ouvrir();
    }
    if (pivot.distance()<8.5){
      pivot.monteDescendre(0.4);
    }
    else if(pivot.distance()>10.5) {
      pivot.monteDescendre(-0.4);
    }
    else{
      pivot.monteDescendre(0);
    }
    }
  
  

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {
    brasTelescopique.AvanceRecule(0);
    pivot.monteDescendre(0);
    brasTelescopique.ouvrir();
  }
  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    return brasTelescopique.distance()>-13.5 && brasTelescopique.distance() <-14.5 && pivot.distance()<8.5 && pivot.distance()>9.5;

  }
}