001/************************ PROJECT ZIXI ************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved.*/
003/* This work is licensed under the terms of the MIT license.  */
004/**************************************************************/
005package com.stuypulse.robot.commands.auton;
006
007import com.stuypulse.robot.Robot;
008import com.stuypulse.robot.constants.FieldConstants;
009
010import org.wpilib.command3.Command;
011
012import bline.field.BLineField;
013import bline.path.Path;
014import java.util.ArrayList;
015import java.util.List;
016
017/**
018 * Implementation of {@link Auton} using BLine paths
019 *
020 * @author Aiden Zhang (https://github.com/aidenzhang21345)
021 */
022public class BLineAuton implements Auton {
023    private final List<Path> paths = new ArrayList<>();
024
025    private final Command command;
026
027    /**
028     * Creates an auton that previews the given paths and runs the given commands
029     *
030     * @param paths Paths to preview
031     * @param name Name of auton command given to scheduler
032     * @param commands Auton command seqeunce
033     */
034    public BLineAuton(Path[] paths, String name, Command... commands) {
035        for (Path path : paths) {
036            this.paths.add(path.copy());
037        }
038
039        this.command = Command.sequence(commands).named(name);
040    }
041
042    @Override
043    public void displayPaths() {
044        for (Path path : paths) {
045            path.setFlipped(!Robot.isBlue());
046            BLineField.drawPath(FieldConstants.FIELD_2D, path);
047        }
048    }
049
050    @Override
051    public void clearPaths() {
052        for (Path path : paths) {
053            FieldConstants.FIELD_2D
054                    .getObject(BLineField.drawPath(FieldConstants.FIELD_2D, path))
055                    .setPoses();
056        }
057    }
058
059    @Override
060    public Command getCommand() {
061        return command;
062    }
063}