package org.strykeforce.thirdcoast.talon;

import com.ctre.CANTalon;
import com.electronwill.nightconfig.core.UnmodifiableConfig;

class PIDTalonConfiguration extends TalonConfiguration {

  private final static String OUTPUT_VOLTAGE_MAX = "output_voltage_max";
  private final static String FORWARD_OUTPUT_VOLTAGE_PEAK = "forward_output_voltage_peak";
  private final static String REVERSE_OUTPUT_VOLTAGE_PEAK = "reverse_output_voltage_peak";
  private final static String FORWARD_OUTPUT_VOLTAGE_NOMINAL = "forward_output_voltage_nominal";
  private final static String REVERSE_OUTPUT_VOLTAGE_NOMINAL = "reverse_output_voltage_nominal";
  private final static String ALLOWABLE_CLOSED_LOOP_ERROR = "allowable_closed_loop_error";
  private final static String NOMINAL_CLOSED_LOOP_VOLTAGE = "nominal_closed_loop_voltage";
  private final static String K_P = "P";
  private final static String K_I = "I";
  private final static String K_D = "D";
  private final static String K_F = "F";
  private final static String I_ZONE = "I_zone";

  private final double outputVoltageMax;
  private final double forwardOutputVoltagePeak;
  private final double reverseOutputVoltagePeak;
  private final double forwardOutputVoltageNominal;
  private final double reverseOutputVoltageNominal;
  private final int allowableClosedLoopError;
  private final double nominalClosedLoopVoltage;
  private final double pGain;
  private final double iGain;
  private final double dGain;
  private final double fGain;
  private final int iZone;

  PIDTalonConfiguration(UnmodifiableConfig config) {
    super(config);
    outputVoltageMax = (double) config.getOptional(OUTPUT_VOLTAGE_MAX).orElse(0.0);
    forwardOutputVoltagePeak = (double) config.getOptional(FORWARD_OUTPUT_VOLTAGE_PEAK).orElse(0.0);
    reverseOutputVoltagePeak = (double) config.getOptional(REVERSE_OUTPUT_VOLTAGE_PEAK).orElse(0.0);
    forwardOutputVoltageNominal = (double) config.getOptional(FORWARD_OUTPUT_VOLTAGE_NOMINAL)
        .orElse(0.0);
    reverseOutputVoltageNominal = (double) config.getOptional(REVERSE_OUTPUT_VOLTAGE_NOMINAL)
        .orElse(0.0);
    allowableClosedLoopError = (int) config.getOptional(ALLOWABLE_CLOSED_LOOP_ERROR).orElse(0);
    nominalClosedLoopVoltage = (double) config.getOptional(NOMINAL_CLOSED_LOOP_VOLTAGE)
        .orElse(0.0); // DisableNominalClosedLoopVoltage, SetNominalClosedLoopVoltage
    pGain = (double) config.getOptional(K_P).orElse(0.0);
    iGain = (double) config.getOptional(K_I).orElse(0.0);
    dGain = (double) config.getOptional(K_D).orElse(0.0);
    fGain = (double) config.getOptional(K_F).orElse(0.0);
    iZone = (int) config.getOptional(I_ZONE).orElse(0);
  }

  @Override
  public void configure(CANTalon talon) {
    if (outputVoltageMax != 0) {
      talon.configMaxOutputVoltage(outputVoltageMax);
    }
    talon.configPeakOutputVoltage(forwardOutputVoltagePeak, reverseOutputVoltagePeak);
    talon.configNominalOutputVoltage(forwardOutputVoltageNominal, reverseOutputVoltageNominal);
    talon.setAllowableClosedLoopErr(allowableClosedLoopError);
    talon.setNominalClosedLoopVoltage(nominalClosedLoopVoltage);
    talon.setPID(pGain, iGain, dGain);
    talon.setF(fGain);
    talon.setIZone(iZone);
    super.configure(talon);
  }

  public double getOutputVoltageMax() {
    return outputVoltageMax;
  }

  public double getForwardOutputVoltagePeak() {
    return forwardOutputVoltagePeak;
  }

  public double getReverseOutputVoltagePeak() {
    return reverseOutputVoltagePeak;
  }

  public double getForwardOutputVoltageNominal() {
    return forwardOutputVoltageNominal;
  }

  public double getReverseOutputVoltageNominal() {
    return reverseOutputVoltageNominal;
  }

  public int getAllowableClosedLoopError() {
    return allowableClosedLoopError;
  }

  public double getNominalClosedLoopVoltage() {
    return nominalClosedLoopVoltage;
  }

  public double getpGain() {
    return pGain;
  }

  public double getiGain() {
    return iGain;
  }

  public double getdGain() {
    return dGain;
  }

  public double getfGain() {
    return fGain;
  }

  public int getiZone() {
    return iZone;
  }

  @Override
  public String toString() {
    return "PIDTalonParameters{" +
        "outputVoltageMax=" + outputVoltageMax +
        ", forwardOutputVoltagePeak=" + forwardOutputVoltagePeak +
        ", reverseOutputVoltagePeak=" + reverseOutputVoltagePeak +
        ", forwardOutputVoltageNominal=" + forwardOutputVoltageNominal +
        ", reverseOutputVoltageNominal=" + reverseOutputVoltageNominal +
        ", allowableClosedLoopError=" + allowableClosedLoopError +
        ", nominalClosedLoopVoltage=" + nominalClosedLoopVoltage +
        ", pGain=" + pGain +
        ", iGain=" + iGain +
        ", dGain=" + dGain +
        ", fGain=" + fGain +
        ", iZone=" + iZone +
        "} " + super.toString();
  }
}

@@ -181,30 +181,29 @@
 ation %7B%0A%0A  p
-rivate
+ublic
  final stati
@@ -253,30 +253,29 @@
 ge_max%22;%0A  p
-rivate
+ublic
  final stati
@@ -339,38 +339,37 @@
 ltage_peak%22;%0A  p
-rivate
+ublic
  final static St
@@ -433,30 +433,29 @@
 e_peak%22;%0A  p
-rivate
+ublic
  final stati
@@ -525,38 +525,37 @@
 ge_nominal%22;%0A  p
-rivate
+ublic
  final static St
@@ -625,30 +625,29 @@
 ominal%22;%0A  p
-rivate
+ublic
  final stati
@@ -715,30 +715,29 @@
 _error%22;%0A  p
-rivate
+ublic
  final stati
@@ -805,30 +805,29 @@
 oltage%22;%0A  p
-rivate
+ublic
  final stati
@@ -845,30 +845,29 @@
 P = %22P%22;%0A  p
-rivate
+ublic
  final stati
@@ -885,30 +885,29 @@
 I = %22I%22;%0A  p
-rivate
+ublic
  final stati
@@ -925,30 +925,29 @@
 D = %22D%22;%0A  p
-rivate
+ublic
  final stati
@@ -965,30 +965,29 @@
 F = %22F%22;%0A  p
-rivate
+ublic
  final stati

Make string constants public