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

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.subsystems.Climber;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Hood;
import frc.robot.subsystems.Indexer;
import frc.robot.subsystems.IntakePivot;
import frc.robot.subsystems.IntakeRollers;
import frc.robot.subsystems.Sensors;
import frc.robot.subsystems.Shooter;

public class RobotContainer {
  private Climber climber;
  private Drivetrain drivetrain;
  private Hood hood;
  private Indexer indexer;
  private IntakePivot intakePivot;
  private IntakeRollers intakeRollers;
  private Shooter shooter;
  private Sensors sensors;

  private CommandXboxController driver;
  private CommandXboxController operator;

  public RobotContainer() {
    configureBindings();

    climber = new Climber();
    drivetrain = new Drivetrain();
    hood = new Hood();
    indexer = new Indexer();
    intakePivot = new IntakePivot();
    intakeRollers = new IntakeRollers();
    shooter = new Shooter();
    sensors = new Sensors();

    driver = new CommandXboxController(0);
    operator = new CommandXboxController(0);
  }

  private void configureBindings() {
    //default commands//
    drivetrain.setDefaultCommand(
      drivetrain.driveArcade(
        driver::getLeftY, driver::getRightX
      )
    );

    climber.setDefaultCommand(climber.stopClimber());
    hood.setDefaultCommand(hood.hoodDown());
    indexer.setDefaultCommand(indexer.stopIndexer());
    intakePivot.setDefaultCommand(intakePivot.intakeUp());
    intakeRollers.setDefaultCommand(intakeRollers.stopIntakeRollers());
    shooter.setDefaultCommand(shooter.stopShooter());

    //controller bindings//
    driver.a().whileTrue(ampUp());
    driver.a().whileFalse(ampScore());
    driver.leftTrigger().whileTrue(smartIntake());

    operator.povUp().onTrue(hood.hoodUp());
    operator.povDown().whileTrue(climber.runClimber());
  }

  public Command getAutonomousCommand() {
    return Commands.print("No autonomous command configured");
  }


  //methods for smart actions//
  public Command ampUp() {
    if (!sensors.getHoodBeamBreak()) {
      return hood.hoodDown()
        .alongWith(shooter.runShooter(.2), indexer.runIndexer(.6));
    } else {
      return hood.hoodUp()
        .alongWith(shooter.stopShooter(), indexer.runIndexer(.2));
    }
  }

  public Command ampScore() {
    if (sensors.getHoodBeamBreak()) {
      return hood.hoodUp()
        .alongWith(shooter.runShooter(1));
    } else {
      return hood.hoodDown()
        .alongWith(shooter.stopShooter());
    }
  }

  public Command smartIntake() {
    if (!sensors.getIndexerBeamBreak()) {
      return intakePivot.intakeDown()
        .alongWith(intakeRollers.runIntakeRollers(1));
    } else {
      return intakePivot.intakeUp()
        .alongWith(intakeRollers.stopIntakeRollers());
    }
  }
}