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