package com.edinarobotics.utils.pid;

import edu.wpi.first.wpilibj.networktables.NetworkTable;
import java.util.Vector;

/**
 * Allows tuning of PID systems through the dashboard using a
 * {@link NetworkTable}.
 * This class handles exchanging data with the dashboard and passing that data
 * to the relevant PIDConfig instance. This class <em>does not</em> save
 * tuning data, it will be lost as soon as the robot is power-cycled.
 */
public class PIDTuningManager {
    private static PIDTuningManager instance;
    private NetworkTable pidTable;
    private Vector pidConfigNames;
    private Vector pidConfigInstances;
    
    private PIDTuningManager(){
        pidTable = NetworkTable.getTable("pid");
        pidConfigNames = new Vector();
        pidConfigInstances = new Vector();
    }
    
    /**
     * Returns the instance of the PIDTuningManager singleton.
     * @return The PIDTuningManager instance.
     */
    public static PIDTuningManager getInstance(){
        if(instance == null){
            instance = new PIDTuningManager();
        }
        return instance;
    }
    
    /**
     * Returns the PIDConfig with the given name. If no PIDConfig with
     * {@code name} exists, a new PIDConfig is created. {@code name} <em>must not</em>
     * contain a comma (",") as this will cause bugs with the dashboard.
     * @param name The name of the requested PIDConfig.
     * @return The PIDConfig instance with the requested {@code name}.
     */
    public PIDConfig getPIDConfig(String name){
        int index = pidConfigNames.indexOf(name);
        if(index >= 0){
            return (PIDConfig) pidConfigInstances.elementAt(index);
        }
        pidConfigNames.addElement(name);
        PIDConfig newConfig = new PIDConfig(name);
        pidConfigInstances.addElement(newConfig);
        return newConfig;
    }
    
    /**
     * Returns a comma-separated String of the names of all existing
     * PIDConfig instances. This is used internally by PIDTuningManager.
     * @return A comma-separated String of the names of all existing PIDConfig
     * instances.
     */
    private String getConfigNames(){
        String names = "";
        if(pidConfigNames.size() > 0){
            names = (String)pidConfigNames.elementAt(0);
        }
        for(int i = 1; i < pidConfigNames.size(); i++){
            names += (","+pidConfigNames.elementAt(i));
        }
        return names;
    }
    
    /**
     * Runs a single iteration of the PID tuning loop. This method should be
     * called periodically during the PID tuning process (for example
     * in the testPeriodic() method of IterativeRobot).
     * This method will handle the NetworkTable data exchange with the dashboard.
     */
    public void runTuning(){
        pidTable.putString("subsystems", getConfigNames());
        PIDConfig pidSystem = getPIDConfig(pidTable.getString("system", "default"));
        pidSystem.setPID(pidTable.getNumber("p", 0), pidTable.getNumber("i", 0),
                pidTable.getNumber("d", 0));
        pidTable.putNumber("value", pidSystem.getValue());
        pidTable.putNumber("setpoint", pidSystem.getSetpoint());
    }
    
    /**
     * Resets all PIDConfig objects to default values. This will undo
     * any tuning performed by the dashboard.
     */
    public void resetAll(){
        for(int i = 0; i < pidConfigInstances.size(); i++){
            ((PIDConfig)pidConfigInstances.elementAt(i)).reset();
        }
    }
}

@@ -105,22 +105,55 @@
 va.util.
-Vector
+Enumeration;%0Aimport java.util.Hashtable
 ;%0A%0A/**%0A 
@@ -593,49 +593,17 @@
 ate 
-Vector pidConfigNames;%0A    private Vector
+Hashtable
  pid
@@ -608,24 +608,16 @@
 idConfig
-Instance
 s;%0A    %0A
@@ -718,20 +718,16 @@
 nfig
-Name
 s = new 
 Vect
@@ -726,57 +726,17 @@
 new 
-Vector();%0A        pidConfigInstances = new Vector
+Hashtable
 ();%0A
@@ -1479,221 +1479,74 @@
    i
-nt index = pidConfigNames.indexOf(name);%0A        if(index %3E= 0)%7B%0A            return (PIDConfig) pidConfigInstances.elementAt(index);%0A        %7D%0A        pidConfigNames.addElement(name);%0A        PIDConfig newConfig =
+f(!pidConfigs.containsKey(name))%7B%0A            pidConfigs.put(name,
  new
@@ -1561,16 +1561,17 @@
 ig(name)
+)
 ;%0A      
@@ -1576,74 +1576,56 @@
     
-pidConfigInstances.addElement(newConfig);%0A        return newConfig
+%7D%0A        return (PIDConfig)pidConfigs.get(name)
 ;%0A  
@@ -1964,36 +1964,73 @@
     
-if(pidConfigNames.size() %3E 0
+Enumeration e = pidConfigs.keys();%0A        if(e.hasMoreElements()
 )%7B%0A 
@@ -2060,34 +2060,22 @@
 ing)
-pidConfigNames.e
+e.nextE
 lement
-At(0
+(
 );%0A 
@@ -2095,53 +2095,33 @@
     
-for(int i = 1; i %3C pidConfigNames.size(); i++
+while(e.hasMoreElements()
 )%7B%0A 
@@ -2144,39 +2144,35 @@
  += 
-(
 %22,%22+
-pidConfigNames.e
+((String)e.nextE
 lement
-At(i
+(
 ));%0A
@@ -2616,85 +2616,164 @@
     
-PIDConfig pidSystem = getPIDConfig(pidTable.getString(%22system%22, %22default%22));%0A
+String systemName = pidTable.getString(%22system%22, %22%22);%0A        if(!systemName.equals((%22%22)))%7B%0A            PIDConfig pidSystem = getPIDConfig(systemName);%0A    
     
@@ -2857,32 +2857,36 @@
                 
+   
+ 
 pidTable.getNumb
@@ -2894,24 +2894,28 @@
 r(%22d%22, 0));%0A
+    
         pidT
@@ -2961,32 +2961,36 @@
 lue());%0A        
+    
 pidTable.putNumb
@@ -3026,24 +3026,34 @@
 etpoint());%0A
+        %7D%0A
     %7D%0A    %0A 
@@ -3227,53 +3227,67 @@
 for(
-int i = 0; i %3C pidConfigInstances.size(); i++
+Enumeration e = pidConfigs.elements(); e.hasMoreElements();
 )%7B%0A 
@@ -3313,38 +3313,22 @@
 fig)
-pidConfigInstances.e
+e.nextE
 lement
-At(i
+(
 )).r

Change PIDTuningManager to use an internal Hashtable instead of Vectors.