001/************************* PROJECT RON *************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */
003/* Use of this source code is governed by an MIT-style license */
004/* that can be found in the repository LICENSE file.           */
005/***************************************************************/
006package tools.PathplannerFlip.PathFlip;
007
008import java.io.File;
009import java.io.IOException;
010import java.util.ArrayList;
011import java.util.List;
012
013import com.fasterxml.jackson.databind.JsonNode;
014import com.fasterxml.jackson.databind.ObjectMapper;
015import com.fasterxml.jackson.databind.node.ObjectNode;
016
017import tools.ToolClasses.VarArgumentedTool;
018
019/**
020 * <h2>Pathplanner Path Flipper</h2>
021 * <p>Flips paths of your choice across the y coordinate axis</p>
022 * <p>This class extends {@link VarArgumentedTool} and takes in a list of path names as arguments.
023 * It reads each .path file, flips the coordinates, and saves a new .path file with " Flipped" appended to the original name.</p>
024 */
025public final class PathplannerPathFlip extends VarArgumentedTool<String> {
026    public static final double FIELD_HEIGHT = 8.0;
027    public static final String PATH_DIR = "./src/main/deploy/pathplanner/paths/";
028
029    public static final ObjectMapper mapper = new ObjectMapper();
030
031    public PathplannerPathFlip() {
032        super(String.class, "The paths to flip over the y axis.");
033    }
034
035    @Override
036    protected void execute(List<String> arguments) {
037        if (arguments.isEmpty()) {
038            System.err.println("Missing path argument.");
039            return;
040        }
041
042        try {
043            flipPaths(arguments.toArray(new String[0]));
044        } catch (IOException e) {
045            e.printStackTrace();
046        }
047    }
048
049    public static boolean fileValid(File file) {
050        return file.exists() && file.isFile() && file.canRead();
051    }
052
053    public static String[] flipPaths(String ...paths) throws IOException {
054        final ArrayList<String> flippedPaths = new ArrayList<>();
055        for (String pathName : paths) {
056            final File pathFile = new File(PATH_DIR + pathName + ".path");
057            if (!fileValid(pathFile)) {
058                logPath(FLIP_STATUS.INVALID, pathName);
059                continue;
060            }
061
062            final ObjectNode pathObj = (ObjectNode) mapper.readTree(pathFile);
063
064            JsonNode waypoints = pathObj.path("waypoints");
065            if (waypoints.isMissingNode()) {
066                logPath(FLIP_STATUS.MISSING_FIELDS, pathName);
067                continue;
068            }
069
070            for (JsonNode waypoint : waypoints) {
071                flipWaypoint((ObjectNode) waypoint, "anchor");
072                flipWaypoint((ObjectNode) waypoint, "prevControl");
073                flipWaypoint((ObjectNode) waypoint, "nextControl");
074            }
075
076            JsonNode rotationTargets = pathObj.get("rotationTargets");
077            if (rotationTargets != null) {
078                for (JsonNode target : rotationTargets) {
079                    ObjectNode targetObj = (ObjectNode) target;
080                    double flipped = -targetObj.path("rotationDegrees").asDouble();
081                    targetObj.put("rotationDegrees", flipped);
082                }
083            }
084
085            JsonNode goalEndState = pathObj.get("goalEndState");
086            if (goalEndState != null) {
087                double flipped = -goalEndState.path("rotation").asDouble();
088                ((ObjectNode) goalEndState).put("rotation", flipped);
089            }
090
091            JsonNode idealStartingState = pathObj.get("idealStartingState");
092            if (idealStartingState != null) {
093                double flipped = -idealStartingState.path("rotation").asDouble();
094                ((ObjectNode) idealStartingState).put("rotation", flipped);
095            }
096
097            JsonNode pointTowardsZones = pathObj.get("pointTowardsZones");
098            if (pointTowardsZones != null) {
099                for (JsonNode zone : pointTowardsZones) {
100                    flipWaypoint((ObjectNode) zone, "fieldPosition");
101                }
102            }
103
104            final File flippedFile = new File(PATH_DIR + pathName + " Flipped.path");
105            mapper.writerWithDefaultPrettyPrinter().writeValue(flippedFile, pathObj);
106            logPath(FLIP_STATUS.FLIPPED, pathName);
107            flippedPaths.add(pathName + " Flipped");
108        }
109        return flippedPaths.toArray(new String[0]);
110    }
111
112    public static void flipWaypoint(ObjectNode parent, String fieldName) {
113        JsonNode node = parent.get(fieldName);
114        if (node == null || node.isNull())
115            return;
116
117        ObjectNode point = (ObjectNode) node;
118        double flippedY = FIELD_HEIGHT - point.get("y").asDouble();
119        point.put("y", flippedY);
120    }
121
122            private enum FLIP_STATUS {
123        FLIPPED,
124        INVALID,
125        MISSING_FIELDS
126    }
127
128    public static void logPath(FLIP_STATUS status, String pathName) {
129        final String RESET = "\u001B[0m";
130        final String RED = "\u001B[31m";
131        final String GREEN = "\u001B[32m";
132        final String YELLOW = "\u001B[33m";
133
134        String coloredPathWithQuotes = YELLOW + "'" + pathName + "'" + RESET;
135        String coloredPathWithoutQuotes = YELLOW + pathName + RESET;
136
137        String message = switch (status) {
138            case FLIPPED -> GREEN + "Saved to " + YELLOW + "'" + coloredPathWithoutQuotes + YELLOW + " Flipped.path'" + GREEN + " successfully!" + RESET;
139            case INVALID -> coloredPathWithQuotes + RED + " is an invalid path." + RESET;
140            case MISSING_FIELDS -> coloredPathWithQuotes + RED + " is missing required fields." + RESET;
141        };
142
143        System.out.println(message);
144    }
145}