package org.lunatecs316.frc2013;

import edu.wpi.first.wpilibj.Preferences;
import java.util.Vector;

/**
 * Robot constants manager
 *
 * @author domenicpaul
 */
public class Constants {

    static {
        constants = new Vector();
    }

    public static final Constant kJoystickDeadband = new Constant("JoystickDeadband", 0.2);

    public static final Constant kDriveEncoderTicksPerRot = new Constant("DriveEncoderTicksPerRot", 360.0);
    public static final Constant kDriveWheelDiameter = new Constant("DriveWheelDiameter", 6.0);
    public static final Constant kDriveWheelBaseWidth = new Constant("DriveWheelBaseWidth", 20.0);

    public static final Constant kDriveGyroP = new Constant("DriveGyroP", 0.0);
    public static final Constant kDriveGyroI = new Constant("DriveGyroI", 0.0);
    public static final Constant kDriveGyroD = new Constant("DriveGyroD", 0.0);

    public static final Constant kDriveEncoderP = new Constant("DriveEncoderP", 0.0004);
    public static final Constant kDriveEncoderI = new Constant("DriveEncoderI", 0.0);
    public static final Constant kDriveEncoderD = new Constant("DriveEncoderD", 0.00005);

    public static final Constant kPickupArmSpeed = new Constant("PickupArmSpeed", 1.0);
    public static final Constant kPickupBeltSpeed = new Constant("PickupBeltSpeed", 1.0);

    public static final Constant kShooterTopPosition = new Constant("ShooterTopPosition", 3.2);
    public static final Constant kShooterMidPosition = new Constant("ShooterMidPosition", 3.1);
    public static final Constant kShooterLoadPosition = new Constant("ShooterLoadPosition", 1.85);

    public static final Constant kShooterTargetSpeed = new Constant("ShooterTargetSpeed", 3975.0);
    public static final Constant kShooterMinFiringSpeed = new Constant("ShooterMinFiringSpeed", 3600.0);

    public static final Constant kShooterSpeedP = new Constant("ShooterSpeedP", -0.005);
    public static final Constant kShooterSpeedI = new Constant("ShooterSpeedI", 0.0);
    public static final Constant kShooterSpeedD = new Constant("ShooterSpeedD", 0.0);

    public static final Constant kShooterAngleP = new Constant("ShooterAngleP", 21.5);
    public static final Constant kShooterAngleI = new Constant("ShooterAngleI", 0.0);
    public static final Constant kShooterAngleD = new Constant("ShooterAngleD", 0.0);

    public static final Constant kDashboardUpdateFrequency = new Constant("DashboardUpdateFrequency", 10);

    public static Vector constants;
    private static Preferences prefs = Preferences.getInstance();

    public static class Constant {
        private String m_name;
        private double m_value;

        private Constant(String name, double value) {
            m_name = name;
            m_value = value;

            Constants.constants.addElement(this);
        }

        public void setValue(double value) {
            m_value = value;
        }

        public double getValue() {
            return m_value;
        }

        public String getName() {
            return m_name;
        }
    }

    /**
     * Setup the constant manager for usage in the program
     */
    public static void init() {
       update();
    }

    /**
     * Read the latest constants from the Dashboard
     */
    public static void update() {
        System.out.println("Updating robot constants");

        boolean modified = false;
        for (int i = 0; i < constants.size(); i++) {    // Loop through all constants
            // Get the current constant
            Constant constant = (Constant) constants.elementAt(i);

            String key = constant.getName();
            double oldValue = constant.getValue();

            // Check if the constant is present on SmartDashboard
            if (prefs.containsKey(key)) {
                // Check to see if the value has changed before updating
                double newValue = prefs.getDouble(key, oldValue);
                if (oldValue != newValue) {
                    // Update the value, use the old one if we fail
                    constant.setValue(prefs.getDouble(key, oldValue));
                    modified = true;
                }
            } else {
                System.out.println("Key '" + key + "' does not exist; creating");
                prefs.putDouble(key, oldValue);
            }
        }

        // Save the updated constants to disk
        if (modified) {
            System.out.println("Saving updated constants to disk");
            prefs.save();
        }
    }
}

@@ -269,17 +269,16 @@
 onstant 
-k
 Joystick
@@ -353,33 +353,32 @@
  final Constant 
-k
 DriveEncoderTick
@@ -460,33 +460,32 @@
  final Constant 
-k
 DriveWheelDiamet
@@ -559,25 +559,24 @@
 al Constant 
-k
 DriveWheelBa
@@ -654,33 +654,32 @@
  final Constant 
-k
 DriveGyroP = new
@@ -733,33 +733,32 @@
  final Constant 
-k
 DriveGyroI = new
@@ -816,25 +816,24 @@
 al Constant 
-k
 DriveGyroD =
@@ -892,33 +892,32 @@
  final Constant 
-k
 DriveEncoderP = 
@@ -980,33 +980,32 @@
  final Constant 
-k
 DriveEncoderI = 
@@ -1073,17 +1073,16 @@
 onstant 
-k
 DriveEnc
@@ -1159,25 +1159,24 @@
 al Constant 
-k
 PickupArmSpe
@@ -1250,17 +1250,16 @@
 onstant 
-k
 PickupBe
@@ -1332,33 +1332,32 @@
  final Constant 
-k
 ShooterTopPositi
@@ -1427,33 +1427,32 @@
  final Constant 
-k
 ShooterMidPositi
@@ -1526,25 +1526,24 @@
 al Constant 
-k
 ShooterLoadP
@@ -1625,25 +1625,24 @@
 al Constant 
-k
 ShooterTarge
@@ -1723,25 +1723,24 @@
 al Constant 
-k
 ShooterMinFi
@@ -1824,33 +1824,32 @@
  final Constant 
-k
 ShooterSpeedP = 
@@ -1912,33 +1912,32 @@
  final Constant 
-k
 ShooterSpeedI = 
@@ -2001,25 +2001,24 @@
 al Constant 
-k
 ShooterSpeed
@@ -2083,33 +2083,32 @@
  final Constant 
-k
 ShooterAngleP = 
@@ -2169,33 +2169,32 @@
  final Constant 
-k
 ShooterAngleI = 
@@ -2262,17 +2262,16 @@
 onstant 
-k
 ShooterA
@@ -2348,17 +2348,16 @@
 onstant 
-k
 Dashboar

Remove 'k' from beginning of constant names