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 com.pathplanner.lib.path.PathPlannerPath;
013import java.util.ArrayList;
014import java.util.List;
015
016/**
017 * Implementation of {@link Auton} using PathPlanner paths
018 *
019 * @author Aiden Zhang (https://github.com/aidenzhang21345)
020 */
021public class PathPlannerAuton implements Auton {
022    private final List<PathPlannerPath> paths = new ArrayList<PathPlannerPath>();
023
024    private final Command command;
025
026    /**
027     * Creates an auton that previews the given paths and runs the given commands
028     *
029     * @param paths Paths to preview
030     * @param name Name of auton command given to scheduler
031     * @param commands Auton command seqeunce
032     */
033    public PathPlannerAuton(PathPlannerPath[] paths, String name, Command... commands) {
034        for (PathPlannerPath path : paths) {
035            this.paths.add(path);
036        }
037
038        this.command = Command.sequence(commands).named(name);
039    }
040
041    @Override
042    public void displayPaths() {
043        for (PathPlannerPath path : paths) {
044            FieldConstants.FIELD_2D
045                    .getObject("path: " + path.name)
046                    .setPoses(
047                            Robot.isBlue()
048                                    ? path.getPathPoses()
049                                    : FieldConstants.transformToOppositeAlliance(
050                                            path.getPathPoses()));
051        }
052    }
053
054    @Override
055    public void clearPaths() {
056        for (PathPlannerPath path : paths) {
057            FieldConstants.FIELD_2D.getObject("path: " + path.name).setPoses();
058        }
059    }
060
061    @Override
062    public Command getCommand() {
063        return command;
064    }
065}