auto selection

// 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 java.util.concurrent.CompletableFuture;
import java.util.function.Supplier;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;

/** Add your docs here. */
public class AutosButLazyLoaded {

    /** The future value we care about */
    private CompletableFuture<Command> selectedAutoFuture = CompletableFuture.supplyAsync(this::doNothing);
    
    /** Alliance color; Must be properly set before we can build autos, so we do that */
    private Alliance alliance = DriverStation.Alliance.Red;

    SendableChooser<Supplier<Command>> autoChooser = new SendableChooser<>();

    /** The value last selected on the chooser.  */
    private Supplier<Command> selectedAuto = this::doNothing;
  
  
    public AutosButLazyLoaded(){
        // Add options to our chooser; This could be done manually if we wanted
        SmartDashboard.putData("AutoSelector/chooser",autoChooser);
        autoChooser.setDefaultOption("Select Auto",this::doNothing);
        autoChooser.addOption("Amp", this::ampTwoNote);
        autoChooser.addOption("Center", this::centerTwoNote);
        autoChooser.addOption("Source", this::sourceTwoNote);
    }

    /** Should be checked in Robot.java::autonomousInit
     *  This will return the selected auto, and *will* block the
     *  main thread if necessary for it to complete. However, this should 
     *  almost never happen, but is safely handled internally if it does
     */
    public Command getAutonomousCommand(){
        try{
             return selectedAutoFuture.get();
        }
        catch(Exception e){
            //If the auto cannot build, then we get an error and print it.
            System.err.println("Failed to build auto command ");
            System.err.println(e);
        }
        return new InstantCommand();
    }

    /**
     * The poll operation that watches for updates that affect autos.
     * This should be called in robot.java::DisabledPeriodic. 
     */
    void periodic(){
        //make sure we have our team color: It's necessary to guarantee this before building autos
        if(DriverStation.getAlliance().isEmpty()) return; 

        //Make sure either a default exists or a option was selected
        if(autoChooser.getSelected() == null) return;

        //Just show the status of our current build on the dashboard
        SmartDashboard.putBoolean("AutoSelector/ready",selectedAutoFuture.isDone());

        //If we haven't changed auto or alliance, nothing to do
        if(selectedAuto == autoChooser.getSelected() 
            && alliance == DriverStation.getAlliance().get()
        ){
            return;
        }

        //Save which one we're running now
        selectedAuto = autoChooser.getSelected();
        alliance = DriverStation.getAlliance().get();


        //cancel current computation and start the new one.
        selectedAutoFuture.cancel(true);

        //Run the function that builds the desired auto in the background
        selectedAutoFuture = CompletableFuture.supplyAsync(selectedAuto);
    }

    private Command doNothing(){
        return new InstantCommand();
    }

    // Example autos that have *super* long build times thanks to timer.delay 
    private Command ampTwoNote(){
        System.out.println("starting auto build");
        Timer.delay(3);
        System.out.println("built amp auto");
        return new InstantCommand();
    }
    private Command centerTwoNote(){
        System.out.println("starting auto build");
        Timer.delay(2);
        System.out.println("built center auto");
        return new InstantCommand();
    }
    private Command sourceTwoNote(){
        System.out.println("starting auto build");
        Timer.delay(1.5);
        System.out.println("built source auto");
        return new InstantCommand();
    }

}