// 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 PivotChercheHaut extends CommandBase {
  private BrasTelescopique brasTelescopique;
  private Pivot pivot;
  /** Creates a new PivotChercheHaut. */
  public PivotChercheHaut(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.photocell()){
      brasTelescopique.ouvrir();
      brasTelescopique.Reset();
      brasTelescopique.AvanceRecule(0);
  }
    else{
      brasTelescopique.AvanceRecule(0.3);
    }
    if (pivot.distance()<43.5){
      pivot.monteDescendre(0.4);
    }
    else if(pivot.distance()>44.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 false;
  }
}