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