#include <pidautotuner.h>
#include <prg_342.h>

void setup() {

  PRG_342 prg = PRG_342();
  analogWriteResolution(12);
  analogReadResolution(12);
  prg.safeShutdown(5);  // Sicheres Herunterfahren des Ausgangs

  Serial.begin(115200);

  Serial.println("Starte in 5 Sekunden.");

  delay(5000);

  Serial.println("Start.");

  PIDAutotuner tuner = PIDAutotuner();

  // Set the target value to tune to
  // This will depend on what you are tuning. This should be set to a value within
  // the usual range of the setpoint. For low-inertia systems, values at the lower
  // end of this range usually give better results. For anything else, start with a
  // value at the middle of the range.
  tuner.setTargetInputValue(5000);

  // Set the loop interval in microseconds
  // This must be the same as the interval the PID control loop will run at
  tuner.setLoopInterval(1);

  // Set the output range
  // These are the minimum and maximum possible output values of whatever you are
  // using to control the system (Arduino analogWrite, for example, is 0-255)
  tuner.setOutputRange(0, 4095 * 0.5);

  // Set the Ziegler-Nichols tuning mode
  // Set it to either PIDAutotuner::ZNModeBasicPID, PIDAutotuner::ZNModeLessOvershoot,
  // or PIDAutotuner::ZNModeNoOvershoot. Defaults to ZNModeNoOvershoot as it is the
  // safest option.
  tuner.setZNMode(PIDAutotuner::ZNModeBasicPID);

  // This must be called immediately before the tuning loop
  // Must be called with the current time in microseconds
  tuner.startTuningLoop(micros());

  // Run a loop until tuner.isFinished() returns true
  long microseconds;
  while (!tuner.isFinished()) {

    // This loop must run at the same speed as the PID control loop being tuned
    long prevMicroseconds = microseconds;
    microseconds = micros();

    // Get input value here (temperature, encoder position, velocity, etc)
    double input = prg.getTorque(false, 10);

    // Call tunePID() with the input value and current time in microseconds
    double output = tuner.tunePID(input, microseconds);

    // Set the output - tunePid() will return values within the range configured
    // by setOutputRange(). Don't change the value or the tuning results will be
    // incorrect.
    prg.setOutput(output, false);

    // This loop must run at the same speed as the PID control loop being tuned
    while (micros() - microseconds < 1) delayMicroseconds(1);
  }

  // Turn the output off here.
  prg.setOutput(0);

  // Get PID gains - set your PID controller's gains to these
  double kp = tuner.getKp();
  double ki = tuner.getKi();
  double kd = tuner.getKd();

  Serial.println("#####");
  Serial.print("Kp: ");
  Serial.println(kp);
  Serial.print("Ki: ");
  Serial.println(ki);
  Serial.print("Kd: ");
  Serial.println(kd);
  Serial.println("#####");
  Serial.println("Abgeschlossen");
}

void loop() {
}