// Prozessor Clock 84 MHz;
// ADC Clock 21 MHz;

unsigned long t1;
unsigned long t2;
unsigned long tao;
unsigned long d;
int w;
int y;
int err;
int errAlt = 0;
float TDAC = 0.001;
float Kp = 0.0;
float Ki = 0.0;
float Kd = 0.0;
float UiAlt = 0.0;
int RegOut = 0;


void setup() {
  REG_ADC_MR = (REG_ADC_MR & 0xFFF0FFFF) | 0x00020000;
  Serial.begin(115200, SERIAL_8N1);                 // Die serielle Kommunikation findet mit der Baudrate 250000 bit/s statt; ueber die "Programming USB Port"
  while (!Serial);

  //SerialUSB.begin(115200);            // Zur Verwendung der "Native USB Port"
  //while(!SerialUSB);

  analogReadResolution(12);             // Standardmaessig ist die Aufloesung auf 8 Bit eingestellt (Spannungswerte zwischen 0-255), durch die Erhoehung erreicht man Werte zwischen 0-4095
  analogWriteResolution(12);

  // Funktion waehrend des Setups um sicherzustellen, dass die serielle Kommunikation stattfindet
  Serial.println('a');
  char c = 'b';
  while (c != 'a')
  {
    c = Serial.read();              // Lese den Serialport bis die Buchstabe 'a' empfangen wird
  }
}

unsigned long getDelay() {          // Funktion zum Ablesen der Verzoegerung in Mikrosekunden anhand der Regelungsperiode
  // Da die Regelungsperiode in Sekunden angegeben wird, muss sie in Microsekunden konvertiert werden
  return TDAC * 1000000;
}

void getGuiVariablen() {// Funktion zum Ablesen der in den GUI von Matlab eingegebenen Variablen zur Bildung der Funktion vom PID-Regler
  // Naemlich, die Regelungsperiode TDAC; Kp; Ki; Kd;
  // Falls die Variablen nicht eingegeben werden, wird die Regelungsperiode auf 1 ms gestzt und die Funktion des Reglers uebersprungen
  String s = "";

  Serial.println("T=?");          // Schreibe im Serial Buffer T=? damit Matlab mit einem Zahl Antwortet

  while (Serial.peek() == -1) {}  
  // Serial.peek() statt Serial.read(), denn nach dem Ablesen loescht peek() den Buffer nicht, waehrend read() schon

  s = Serial.readStringUntil(';'); // Speichere den kommenden Wert als String bis zu ';'
  TDAC = 0.000068 + s.toFloat();   // Das komplette Main-loop durchzufuehren dauert 68 Mikrosekunden
  s = "";                          // Nach dem Ablesen des ersten Wertes entleere die String-Variable s

  Serial.println("P=?");
  while (Serial.peek() == -1) {}
  s = Serial.readStringUntil(';');
  Kp = s.toFloat();
  s = "";

  Serial.println("I=?");
  while (Serial.peek() == -1) {}
  s = Serial.readStringUntil(';');
  Ki = s.toFloat();
  s = "";

  Serial.println("D=?");

  while (Serial.peek() == -1) {}
  s = Serial.readStringUntil(';');
  Kd = s.toFloat();
  s = "";

}

void ADC_Lesen() {               // Funktion zum Ablesen und Ausgeben der Soll-Werte am Eingang "A1" und Ist-Werte am Eingang "A2"
  y = analogRead(A2);             // Lese den Wert am Eingang vom ADC 2 bzw. Ist-Wert und speichere in y
  w = analogRead(A1);             // Lese den Soll-Wert am ADC 1. und speichere in w
}

int regler (float p, float i, float d, int a, int b) {  // Funktion des Reglers. Dabei ist p=Kp; i=Ki; d=Kd; a: Spannung am Eingang A2 bzw. Soll-Wert; b: Spannung am Eingang A1 bzw. Ist-Wert

  float Up;
  float Ui;
  float Ud;
  float OutPreSat;                      // Variable zur Speicherung des Ausgangs vom Regler vor der Grenzenueberpruefung

  err = a - b;                          // Bilde die Regelabweichung und wandele sie fuer die weiteren Rechnungen in "Float" um
  float e = float(err);
  float eA = float(errAlt);

  Up = p * e;                           // Bilde den P-Anteil
  Ui = UiAlt + (i * TDAC * e);          // Bilde den I-Anteil
  Ud = d * ((e - eA) / TDAC);           // Bilde den D-Anteil
  OutPreSat = Up + Ui + Ud;             // Bilde die Summe des PID-Reglers

  UiAlt = Ui;                           // Aktualisiere den I-Anteil fuer den Zeitpunkt (z-1)
  errAlt = err;                         // Aktualisiere die Regelabweichung fuer den Zeitpunkt (z-1)

  return int(OutPreSat);                  // Gebe den Wert des Reglers als Integer-Wert aus
}

void steuern(int soll, int reg) {         // Funktion zur Steuerung vom DAC und Ueberpruefung von Saettigung

  int sum = soll + reg;
  if (sum > 4095) {               // Ueberpruefe ob der Ausgang vom DAC innerhalb der Grenzen liegt 4095 = +3,19 V; 1 = -3,18 V
    sum = 4095;
  }
  else if (sum < 1) {
    sum = 1;
  }
  else if (1 < sum < 4095) {
    sum = sum;
  }

  analogWrite(DAC1, sum);
}

void loop() {                           // Hauptprogram;

  while (Serial.peek() == -1) {           // Wenn der Serial Buffer leer ist, wird daraus der Wert -1 gelesen
  
    ADC_Lesen();                          // Wenn der Buffer leer ist, mach weiter ohne Daten zu senden

    RegOut = regler(Kp, Ki, Kd, w, y);
    steuern(w, RegOut);

    d = getDelay();

    delayMicroseconds(d);
  }

  char q = Serial.read();
  if (q == 'u') {
    getGuiVariablen();
  }

  else if (q == 'y') {
    Serial.print('y');
    Serial.println(y, DEC);                    // Schreibe den y-Wert in den Serial Buffer
  }

  else if (q == 'w') {
    Serial.print('w');
    Serial.println(w, DEC);                    // Schreibe den y-Wert in den Serial Buffer
  }
}