001package tools.PathplannerFlip.AutonFlip;
002
003import java.io.File;
004import java.io.IOException;
005import java.util.Arrays;
006import java.util.List;
007import java.util.function.Supplier;
008
009import com.fasterxml.jackson.databind.node.ArrayNode;
010import com.fasterxml.jackson.databind.node.ObjectNode;
011
012import tools.ToolClasses.VarArgumentedTool;
013import tools.PathplannerFlip.PathFlip.PathplannerPathFlip;
014
015import static tools.util.AnsiColors.*;
016
017/**
018 * <h2>Pathplanner Auton Flipper</h2>
019 * <p>Flips autons of your choice across the y coordinate axis</p>
020 * <p>This class extends {@link VarArgumentedTool} and takes in a list of auton names as arguments.
021 * It reads each .auto file, flips the paths used in the auton, and saves a new .auto file with " Flipped" appended to the original name.</p>
022 */
023public class PathplannerAutonFlip extends VarArgumentedTool<String> {
024    protected static final String AUTON_DIR = "./src/main/deploy/pathplanner/autos/";
025
026    public PathplannerAutonFlip() {
027        super(String.class, "The autons to flip over the y axis.");
028    }
029
030    public static boolean fileValid(File file) {
031        return file.exists() && file.isFile() && file.canRead();
032    }
033
034    @Override
035    protected void execute(List<String> arguments) {
036        if (arguments.size() <= 0) {
037            throw new IllegalArgumentException("Missing path argument.");
038        }
039
040        for (String autonName : arguments) {
041            final File autonFile = new File(AUTON_DIR + autonName + ".auto");
042            if (!PathplannerPathFlip.fileValid(autonFile)) {
043                logPath(FLIP_STATUS.INVALID, autonName);
044                continue;
045            }
046
047            final ObjectNode autonObj = ((Supplier<ObjectNode>) () -> {
048                try {
049                    return (ObjectNode) PathplannerPathFlip.mapper.readTree(autonFile);
050                } catch (IOException e) {
051                    e.printStackTrace();
052                    return null;
053                }
054            }).get();
055            if (autonObj == null) continue;
056
057            ArrayNode commandArray = (ArrayNode) autonObj
058                .path("command")
059                .path("data")
060                .path("commands");
061
062            String[] pathNames = commandArray.valueStream()
063                .filter(command -> command.path("type").asText().equals("path"))
064                .map(command -> command.path("data").path("pathName").asText())
065                .toArray(String[]::new);
066
067            if (pathNames.length <= 0) {
068                logPath(FLIP_STATUS.NO_PATHS, autonName);
069                continue;
070            }
071
072            try {
073                writeAuton(autonObj, autonName, PathplannerPathFlip.flipPaths(pathNames));
074            } catch (IOException e) {
075                e.printStackTrace();
076            }
077        }
078    }
079
080    public static final ObjectNode blankPathObject = PathplannerPathFlip.mapper.createObjectNode()
081            .put("type", "path")
082            .set("data", PathplannerPathFlip.mapper.createObjectNode());
083
084    public static void writeAuton(ObjectNode autonObj, String name, String[] paths) throws IOException {
085        ObjectNode dataNode = (ObjectNode) autonObj
086                .path("command")
087                .path("data");
088
089        ArrayNode flippedPaths = PathplannerPathFlip.mapper.createArrayNode();
090        Arrays.stream(paths)
091                .map(pathName -> {
092                    ObjectNode pathObj = (ObjectNode) blankPathObject.deepCopy().path("data");
093                    pathObj.put("pathName", pathName);
094                    return (ObjectNode) blankPathObject.deepCopy().set("data", pathObj);
095                })
096                .forEach(flippedPaths::add);
097        dataNode.set("commands", flippedPaths);
098
099        PathplannerPathFlip.mapper
100                .writerWithDefaultPrettyPrinter()
101                .writeValue(new File(AUTON_DIR + name + " Flipped.auto"), autonObj);
102        logPath(FLIP_STATUS.FLIPPED, name);
103    }
104
105        private enum FLIP_STATUS {
106        FLIPPED,
107        INVALID,
108        NO_PATHS
109    }
110
111    public static void logPath(FLIP_STATUS status, String pathName) {
112        String coloredPathWithQuotes = YELLOW + "'" + pathName + "'" + RESET;
113        String coloredPathWithoutQuotes = YELLOW + pathName + RESET;
114
115        String message = switch (status) {
116            case FLIPPED -> GREEN + "Saved to " + YELLOW + "'" + coloredPathWithoutQuotes + YELLOW + " Flipped.auto'" + GREEN + " successfully!" + RESET;
117            case INVALID -> coloredPathWithQuotes + RED + " is an invalid auton." + RESET;
118            case NO_PATHS -> coloredPathWithQuotes + RED + " has no path commands." + RESET;
119        };
120
121        System.out.println(message);
122    }
123}