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.util.auton;
006
007import com.stuypulse.robot.commands.auton.Auton;
008import com.stuypulse.robot.commands.auton.PathPlannerAuton;
009
010import org.wpilib.driverstation.DriverStationErrors;
011
012import com.pathplanner.lib.path.PathPlannerPath;
013import java.io.IOException;
014import java.nio.file.DirectoryStream;
015import java.nio.file.Files;
016import java.nio.file.Path;
017import java.nio.file.Paths;
018import java.util.ArrayList;
019import java.util.Collections;
020import java.util.HashMap;
021import java.util.List;
022import java.util.function.Function;
023import org.littletonrobotics.junction.networktables.LoggedNetworkChooser;
024
025public class PathPlannerUtil {
026
027    public static class PathPlannerConfig {
028
029        private final String name;
030
031        private final Function<PathPlannerPath[], PathPlannerAuton> auton;
032
033        private final String[] paths;
034
035        public PathPlannerConfig(
036                String name, Function<PathPlannerPath[], PathPlannerAuton> auton, String... paths) {
037            this.name = name;
038            this.auton = auton;
039            this.paths = paths;
040            for (String path : paths) {
041                try {
042                    PathPlannerPath.fromPathFile(path);
043                } catch (Exception e) {
044                    DriverStationErrors.reportError(
045                            "Path \""
046                                    + path
047                                    + "\" not found. Did you mean \""
048                                    + PathPlannerUtil.findClosestMatch(
049                                            PathPlannerUtil.getPathFileNames(), path)
050                                    + "\"?",
051                            false);
052                }
053            }
054        }
055
056        private PathPlannerAuton buildPathPlannerAuton() {
057            return auton.apply(loadPaths(paths));
058        }
059
060        public PathPlannerConfig register(LoggedNetworkChooser<Auton> chooser) {
061            chooser.add(name, buildPathPlannerAuton());
062            return this;
063        }
064
065        public PathPlannerConfig registerDefault(LoggedNetworkChooser<Auton> chooser) {
066            chooser.add(name, buildPathPlannerAuton());
067            return this;
068        }
069    }
070
071    /** PATH LOADING ** */
072    public static PathPlannerPath[] loadPaths(String... names) {
073        PathPlannerPath[] output = new PathPlannerPath[names.length];
074        for (int i = 0; i < names.length; i++) {
075            output[i] = load(names[i]);
076        }
077        return output;
078    }
079
080    public static PathPlannerPath load(String name) {
081        try {
082            return PathPlannerPath.fromPathFile(name);
083        } catch (Exception e) {
084            DriverStationErrors.reportError("Path not found.", false);
085            return null;
086        }
087    }
088
089    /** PATH FILENAME CORRECTION ** */
090    public static List<String> getPathFileNames() {
091        // ../../../../../deploy/pathplanner/paths
092        Path path = Paths.get("").toAbsolutePath().resolve("src/main/deploy/pathplanner/paths");
093        ArrayList<String> fileList = new ArrayList<String>();
094        try (DirectoryStream<Path> stream = Files.newDirectoryStream(path, "*.path")) {
095            for (Path file : stream) {
096                fileList.add(file.getFileName().toString().replaceFirst(".path", ""));
097            }
098        } catch (IOException error) {
099            DriverStationErrors.reportError(error.getMessage(), false);
100        }
101        Collections.sort(fileList);
102        return fileList;
103    }
104
105    public static String findClosestMatch(List<String> paths, String input) {
106        double closestValue = 10.0;
107        String matching = "";
108        for (String fileName : paths) {
109            HashMap<Character, Integer> fileChars = countChars(fileName.toCharArray());
110            HashMap<Character, Integer> inputChars = countChars(input.toCharArray());
111            double proximity = compareNameProximity(fileChars, inputChars);
112            closestValue = Math.min(proximity, closestValue);
113            if (proximity == closestValue) {
114                matching = fileName;
115            }
116        }
117        return matching;
118    }
119
120    public static HashMap<Character, Integer> countChars(char[] chars) {
121        HashMap<Character, Integer> letterMap = new HashMap<>();
122        for (char i = 'a'; i <= 'z'; i++) letterMap.put(i, 0);
123        for (char i = 'a'; i <= 'z'; i++) letterMap.put(i, 0);
124        letterMap.put('(', 0);
125        letterMap.put(' ', 0);
126        letterMap.put(')', 0);
127        for (char letter : chars) {
128            if (letterMap.containsKey(letter)) {
129                letterMap.put(letter, letterMap.get(letter));
130            } else {
131                letterMap.put(letter, 1);
132            }
133        }
134        return letterMap;
135    }
136
137    public static double compareNameProximity(
138            HashMap<Character, Integer> list1, HashMap<Character, Integer> list2) {
139        double proximity = 0.0;
140        int list1sum = 0, list2sum = 0;
141        for (char key : list1.keySet()) {
142            if (!list2.containsKey(key)) {
143                proximity += 0.1;
144                continue;
145            }
146            proximity += 0.05 * Math.abs(list1.get(key) - list2.get(key));
147        }
148        for (char key : list2.keySet()) {
149            if (!list1.containsKey(key)) {
150                proximity += 0.1;
151                continue;
152            }
153            proximity += 0.05 * Math.abs(list1.get(key) - list2.get(key));
154        }
155        for (int count : list1.values()) list1sum += count;
156        for (int count : list2.values()) list2sum += count;
157        proximity += 0.4 * Math.abs(list2sum - list1sum);
158        return proximity;
159    }
160}