#include <prg_342.h>
#define MEAS_SAMPLES 4000
#define MEAS_TIME 100
#define K_FAKTOR 1.0995380532183

// Initialisiere die PRG_342-Klasse
PRG_342 prg = PRG_342(K_FAKTOR);

//Messung / Logging
bool LOGGING = false;
int measTorque[MEAS_SAMPLES];
int measAngle[MEAS_SAMPLES];
unsigned int measSetpoint[MEAS_SAMPLES];
unsigned int measMillis[MEAS_SAMPLES];
unsigned int measVolt[MEAS_SAMPLES];
unsigned int logCounter;
unsigned long timer;
unsigned long diff;
float ALPHA = 0.0008;

// Deklaration der Variablen für die Steuerung und Messung
int angle = 0;      // Aktueller Winkel
int lastAngle = 0;  // Letzter gemessener Winkel
int actTorque = 0;  // Aktuelles Drehmoment
int setpoint = 0;   // Sollwert für Drehmoment
float tolerance = 0.0;
//int tolerance = 500;
float lowerBound;
float upperBound;
static int lastOutput = 0;
unsigned int manVolt = 0;
unsigned char CW;
unsigned char Richtungswechsel;

// PID-Reglerparameter
//float Kp = 5.15;
//float Ki = 0.02;
//float Kd = 2273.47;
float Kp = 0;
float Ki = 0.010;
float Kd = 0;
float I_ALPHA = 0.0;
float I_old = 0;
int Pout;
int Iout;
int Dout;

int error;  // Fehler zwischen Soll- und Istwert (Regeldifferenz e)

// Steuervariablen
bool OFFSET_ON = true;
bool manOut = false;          // Manuelle Ausgabe aktivieren/deaktivieren
bool PID = false;             // PID-Regelung ein-/ausschalten
unsigned char PID_DELAY = 0;  // Verzögerung zwischen PID-Regelungen in ms
unsigned int Imax = 217;      // Maximalstrom in mA
bool PRINT_DEBUG = true;      // Debug-Ausgaben ein-/ausschalten
bool DEBUG_MODE = false;      // Debug-Modus ein-/ausschalten
bool ARDUINO_PLOTTER = false;

// Variablen für PID-Berechnungen
float integral = 0;     // Integralterm
int derivative;         // Differenzialterm
int previousError = 0;  // Vorheriger Fehler
unsigned long lastTime = 0;
unsigned long currentTime;
unsigned long timeChange;

// Initialisierung der verschiedenen Timer
unsigned long timer1;
unsigned long timer2;
unsigned long PIDtimer;
unsigned long logTimer;

float RotSpeed;                       // Rotationsgeschwindigkeit
unsigned int MilliAmpPerSecond = 25;  // Milliampere pro Sekunde
unsigned int maxChange;               // Maximale Änderung pro Schleifendurchlauf

char serBuffer[4];  // Buffer für serielle Eingaben

void setup() {

  for (int i = 0; i < MEAS_SAMPLES; i++) {
    measTorque[i - 1] = 0;
    measAngle[i - 1] = 0;
    measSetpoint[i - 1] = 0;
    measMillis[i - 1] = 0;
    measVolt[i - 1] = 0;
  }

  analogWriteResolution(12);
  analogReadResolution(12);

  Serial.begin(115200);  // Beginne serielle Kommunikation

  prg.safeShutdown(5);  // Sicheres Herunterfahren des Ausgangs
  //prg.AutoDecog();

  // Initialisiere Timer mit der aktuellen Zeit
  timer1 = timer2 = PIDtimer = logTimer = millis();

  // Lese den initialen Winkel
  angle = prg.getAngle();
  lastAngle = angle;
}

void Datalogger() {
  if (LOGGING) {
    if (millis() - logTimer >= MEAS_TIME) {
      logTimer = millis();
      logCounter++;

      measTorque[logCounter] = actTorque;
      measAngle[logCounter] = angle;
      measSetpoint[logCounter] = setpoint;
      measMillis[logCounter] = logCounter * MEAS_TIME;
      measVolt[logCounter] = prg.getOutVolt();
    }
  }
}

void loop() {
  angle = prg.getAngle();
  actTorque = prg.getTorque(true, ALPHA);
  // Aktualisiere den analogen Ausgang für den aktuellen Drehmoment
  prg.updateActOut(actTorque);

  // Prüfe, ob der Debug-Modus aktiviert ist
  DEBUG_TIMER();  // Aktualisierung des Debug-Timers
  if (DEBUG_MODE) {
    DEBUG_PRINTER();  // Debug Informationen ausgeben
  }

  if (ARDUINO_PLOTTER) {
    Arduino_Plotter();
  }

  Datalogger();

  // Verarbeitung der seriellen Schnittstelle
  COMMUNICATION_HANDLER();

  // Rotationsgeschwindigkeit aktualisieren
  SPEEDCHECK();


  PID_CONTROL();  // Ausführung der PID-Regelung

  // setze isRunning zurück, damit es durch den Interrupt des Drehgebers wieder gesetzt werden kann
  prg.isRunning = false;
}


void COMMUNICATION_HANDLER() {
  // Verarbeite eingehende Befehle von der seriellen Schnittstelle
  if (Serial.available() > 0) {
    delay(10);
    unsigned int SerIn = Serial.read();
    switch (SerIn) {
      case 'a':
        // Drehwinkel und Drehmoment ausgeben
        Serial.print((float)angle / 1000, 1);
        Serial.print(";");
        Serial.println((float)actTorque / 1000, 1);
        break;

      case 'e':
        // safeShutdown ausführen
        prg.safeShutdown(10);
        break;

      case 't':
        // Drehmoment nullen
        prg.tareTorque();
        break;

      case 'w':
        // Winkel nullen
        prg.tareAngle();
        break;

      case 'o':
        // Offset für Drehmoment deaktivieren
        OFFSET_ON = !OFFSET_ON;
        break;

        // Einstellen der Parameter Kp, Ki und Kd
      case 'p':
        delay(10);
        Serial.readBytes(serBuffer, 4);
        Kp = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001;
        Serial.print("Kp: ");
        Serial.println(Kp);
        break;

      case 'i':
        delay(10);
        Serial.readBytes(serBuffer, 4);
        Ki = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001;
        Serial.print("Ki: ");
        Serial.println(Ki);
        break;

      case 'd':
        delay(10);
        Serial.readBytes(serBuffer, 4);
        Kd = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001;
        Serial.print("Kd: ");
        Serial.println(Kd);
        break;

      case 'j':
        delay(10);
        Serial.readBytes(serBuffer, 4);
        I_ALPHA = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001;
        Serial.print("I_ALPHA: ");
        Serial.println(I_ALPHA);
        break;

      case 's':
        // Soll-Wert Einstellung
        delay(10);
        Serial.readBytes(serBuffer, 4);
        setpoint = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001;
        Serial.print("setpoint: ");
        Serial.println(setpoint);
        break;

      case 'm':
        // Spannungsausgabe auf manuell stellen
        manOut = !manOut;
        break;

      case 'v':
        // Ausgangsspannung einstellen (manOut muss auf true sein)
        if (manOut) {
          Serial.readBytes(serBuffer, 4);
          manVolt = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]);
          //prg.setOutput((float)manVolt * 0.1575, false);
          float outVal = (float)manVolt * 0.1575;
          prg.ActOut = outVal;
          analogWrite(prg.TORQUE_OUT_PIN, outVal);
        }
        break;

      case 'x':
        // Regelung ein-/ausschalten
        PID = !PID;
        error = 0;
        previousError = 0;
        integral = 0;
        break;

      case 'r':
        // PID-Parameter zurücksetzen
        error = 0;
        previousError = 0;
        integral = 0;
        derivative = 0;
        Pout = 0;
        Iout = 0;
        Dout = 0;
        break;

      case 'f':
        // Dynamic Decogging ausführen
        prg.DynDecog(prg.ActOut);
        break;

      case 'k':
        // Einstellen des maximalen Stromanstiegs
        delay(10);
        Serial.readBytes(serBuffer, 4);
        MilliAmpPerSecond = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]);
        Serial.print("Max Milliampere per second: ");
        Serial.println(MilliAmpPerSecond);
        break;

      case 'b':
        // DEBUG_MODE switch
        DEBUG_MODE = !DEBUG_MODE;
        break;

      case 'l':
        LOGGING_HANDLER();
        break;

      case 'y':
        // Automatic Measurement
        timer = millis();
        diff = millis() - timer;
        LOGGING_HANDLER();
        PID = true;
        while (diff <= 150000) {
          angle = prg.getAngle();
          actTorque = prg.getTorque(true, ALPHA);
          prg.updateActOut(actTorque);

          Datalogger();

          /*if (diff > 20000 && diff <= 50000) {
            setpoint = 5000;
          }
          if (diff > 50000 && diff <= 80000) {
            setpoint = 10000;
          }
          if (diff > 80000 && diff <= 110000) {
            setpoint = 15000;
          }
          if (diff > 110000 && diff <= 140000) {
            setpoint = 5000;
          }*/

          if (angle > 0 && angle <= 45000) {
            setpoint = 10000;
          }
          if (angle > 45000 && angle <= 65000) {
            setpoint = 15000;
          }
          if (angle > 65000 && angle <= 90000) {
            setpoint = 10000;
          }


          SPEEDCHECK();

          PID_CONTROL();

          diff = millis() - timer;

          Serial.print("elapsed Time: ");
          Serial.print((float)diff / 1000, 3);
          Serial.print("     actual Angle: ");
          Serial.println((float)angle / 1000, 3);
        }
        LOGGING_HANDLER();
        PID = false;
        break;

      default:
        // Unbekannte Befehle bearbeiten
        Serial.print("Unbekannter Befehl: ");
        Serial.println(SerIn);
        break;
    }
  }
}

void printData() {
  Serial.print("Kp:");
  Serial.print(";");
  Serial.println(Kp, 3);
  Serial.print("Ki:");
  Serial.print(";");
  Serial.println(Ki, 3);
  Serial.print("Kd:");
  Serial.print(";");
  Serial.println(Kd, 3);
  Serial.print("I_ALPHA:");
  Serial.print(";");
  Serial.println(I_ALPHA, 6);
  Serial.print("Drehrichtung:");
  Serial.print(";");
  if (CW) {
    Serial.println("CW");
  } else {
    Serial.println("CCW");
  }

  Serial.println("Zeit in Sekunden;Winkel;Soll-Drehmoment;Ist-Drehmoment;Erregerspannung;Winkel in Grad;Soll-Drehmoment in Nm;Ist-Drehmoment in Nm;Erregerspannung in V");
  for (int i = 0; i < MEAS_SAMPLES; i++) {
    if (measMillis[i] > 0) {
      Serial.print((float)measMillis[i] / 1000.0, 2);
      Serial.print(";");
      Serial.print(measAngle[i]);
      Serial.print(";");
      Serial.print(measSetpoint[i]);
      Serial.print(";");
      Serial.print(measTorque[i]);
      Serial.print(";");
      Serial.print(measVolt[i]);
      Serial.print(";");
      Serial.print(abs((float)measAngle[i] / 1000.0), 3);
      Serial.print(";");
      Serial.print(abs((float)measSetpoint[i] / 1000.0), 3);
      Serial.print(";");
      Serial.print(abs((float)measTorque[i] / 1000.0), 3);
      Serial.print(";");
      Serial.println(abs((float)measVolt[i] / 1000.0), 3);
    }
  }
  for (int i = 0; i < MEAS_SAMPLES; i++) {
    measTorque[i - 1] = 0;
    measAngle[i - 1] = 0;
    measSetpoint[i - 1] = 0;
    measMillis[i - 1] = 0;
    measVolt[i - 1] = 0;
  }
}

void LOGGING_HANDLER() {
  // LOGGING switch
  LOGGING = !LOGGING;
  if (LOGGING) {
    logCounter = 0;
  } else {
    printData()
  }
}

void PID_CONTROL() {
  lowerBound = (float)setpoint - ((float)setpoint * tolerance);
  upperBound = (float)setpoint + ((float)setpoint * tolerance);
  //lowerBound = (float)setpoint - tolerance;
  //upperBound = (float)setpoint + tolerance;
  // Timing des PID-Reglers
  if (millis() - PIDtimer >= PID_DELAY) {

    if (Richtungswechsel) {
      integral = 0;
      derivative = 0;
      Richtungswechsel = 0;
    }

    PIDtimer = millis();                  // Setze den Timer zurück
                                          // Aktualisierung des Zeitstempels und Berechnung der vergangenen Zeit seit dem letzten Update
    currentTime = millis();               // Lese die aktuelle Zeit in Millisekunden
    timeChange = currentTime - lastTime;  // Zeitdifferenz zum letzten Aufruf
    lastTime = currentTime;               // Setze den letzten Zeitstempel auf die aktuelle Zeit für den nächsten Durchlauf


    // Überprüfe, ob die PID-Regelung aktiviert ist, keine manuelle Ausgabe erfolgt,
    // das gemessene Drehmoment die Schwelle überschreitet und die Geschwindigkeit hoch genug ist
    // ODER das gemessene Drehmoment den Sollwert übersteigt (Antrieb auf Block)
    if (PID && !manOut && ((abs(actTorque) >= 5000 || abs(RotSpeed) >= 400) || (abs(actTorque) > setpoint)) && (abs(actTorque) < lowerBound || abs(actTorque) > upperBound)) {

      // Aktuelle Werte auslesen
      //actTorque = abs(prg.getTorque(true, 100));  // Aktuelles Drehmoment
      //angle = abs(prg.getAngle());               // Aktueller Winkel

      // Berechnung der Regeldifferenz
      error = abs(setpoint) - abs(actTorque);  // Differenz zwischen Sollwert und tatsächlich gemessenem Drehmoment

      // Berechnung des proportionalen Anteils der PID-Regelung
      Pout = Kp * error;  // Proportionaler Output basiert auf dem aktuellen Fehler und dem Proportional-Faktor Kp
                          /*
      // Berechnung des integralen Anteils der PID-Regelung
      integral += error * ((float)timeChange / 1000.0);   // Addition des Fehlers über die Zeit (Integration des Fehlers)
      integral = constrain(integral, -4095000, 4095000);  // Begrenzung des integralen Anteils, um Windup zu vermeiden
      Iout = Ki * integral; // Berechnung des integralen Output basierend auf dem Integral und dem Integral-Faktor Ki
      */

      // Berechnung des adaptiv gewichteten integralen Anteils
      float weight = 1.0 + (1 - exp(-PHI_ALA * abs(error)));      // Gewichtungsfaktor abhängig vom Fehler
      integral += weight * error * ((float)timeChange / 1000.0);  // Anpassung der Integration des Fehlers
      integral = constrain(integral, -4095000, 4095000);          // Begrenzung des integralen Anteils, um Windup zu vermeiden
      Iout = Ki * integral;

      // Berechnung des differenziellen Anteils der PID-Regelung
      derivative = (error - previousError) / ((float)timeChange / 1000.0);  // Berechnung der Änderungsrate des Fehlers
      Dout = Kd * derivative;                                               // Differenzieller Output basiert auf der Änderungsrate des Fehlers und dem Differenzial-Faktor Kd

      // Berechnung des gesamten PID-Output
      int output = Pout + Iout + Dout;  // Summe der einzelnen PID-Komponenten

      // Begrenzung des Ausganges, um Cogging zu vermeiden
      // Der Strom darf nicht auf 0 sinken, während die Hysteresebremse stillsteht
      output = constrain(output, 0, 4095);

      // Begrenzung des Stromanstieges, um Cogging vorzubeugen
      //maxChange = ((float)MilliAmpPerSecond * 4095.0 * (float)PID_DELAY) / ((float)Imax * 1000);  // Umrechnung des eingegebenen Stromwertes / Sekunde in einen binären Wert, angepasst auf den Ausgang
      /*maxChange = 1;
      if (abs(output - lastOutput) > maxChange) {
        if (output > lastOutput) {
          output = lastOutput + maxChange;  // Erhöht den Output
        } else {
          output = lastOutput - maxChange;  // Verringert den Output
        }
      }*/

      // Den Wert output am Ausgang (an die Hysteresebremse) anlegen
      prg.setOutput(output, false);
      lastOutput = output;  // Aktualisiere den letzten Output-Wert

      // Aktualisierung des vorherigen Fehler
      previousError = error;
    }
  }
}


void SPEEDCHECK() {
  // Überprüfen der Rotationsgeschwindigkeit
  if (millis() - timer2 >= 200) {
    timer2 = millis();
    int deltaAngle = angle - lastAngle;
    RotSpeed = deltaAngle * 5;  // Umrechnen in Winkel pro Sekunde
    lastAngle = angle;

    if (RotSpeed > 0) {
      if (CW == 0) {
        Richtungswechsel = 1;
      }
      CW = 1;
    } else if (RotSpeed < 0) {
      if (CW == 1) {
        Richtungswechsel = 1;
      }
      CW = 0;
    }
  }
}

void DEBUG_TIMER() {
  // Timer für Debug-Ausgaben
  if (millis() - timer1 >= 500) {
    timer1 = millis();
    PRINT_DEBUG = true;
  } else {
    PRINT_DEBUG = false;
  }
}

void DEBUG_PRINTER() {
  // Debug-Informationen ausgeben
  if (PRINT_DEBUG) {
    Serial.println("####################");
    Serial.print("Winkel: ");
    Serial.println((float)angle / 1000, 3);
    Serial.print("Drehmoment: ");
    Serial.println((float)actTorque / 1000, 3);
    Serial.print("Geschwindigkeit: ");
    Serial.println(RotSpeed);
    Serial.print("Drehrichtung: ");
    Serial.println(CW);
    Serial.print("Ausgangsspannung: ");
    Serial.println((float)prg.getOutVolt() / 1000, 3);
    Serial.print("Setpoint: ");
    Serial.println((float)setpoint / 1000, 3);
    Serial.print("Error: ");
    Serial.println((float)error / 1000, 3);
    Serial.print("Kp: ");
    Serial.println(Kp, 3);
    Serial.print("Ki: ");
    Serial.println(Ki, 3);
    Serial.print("Kd: ");
    Serial.println(Kd, 3);
    Serial.print("I_ALPHA:");
    Serial.println(I_ALPHA);
    Serial.print("Pout: ");
    Serial.println(Pout);
    Serial.print("Iout: ");
    Serial.println(Iout);
    Serial.print("Dout: ");
    Serial.println(Dout);
    Serial.print("output: ");
    Serial.println((unsigned int)((float)prg.getOutVolt() * 0.1575));
    Serial.print("Integral: ");
    Serial.println(integral);
    Serial.print("PID: ");
    Serial.println(PID);
    Serial.print("manOut: ");
    Serial.println(manOut);
    Serial.print("manVolt: ");
    Serial.println(manVolt);
    Serial.print("Max mA / s (SET): ");
    Serial.println(MilliAmpPerSecond);
    Serial.print("Max mA / s (ACT): ");
    Serial.println((Imax * maxChange * 1000) / (4095 * PID_DELAY));
    Serial.print("maxChange: ");
    Serial.println(maxChange);
    Serial.print("isRunning: ");
    Serial.println(prg.isRunning);
    Serial.print("LOGGING: ");
    Serial.println(LOGGING);
    Serial.println("####################");
  }
}

void Arduino_Plotter() {
  if (PRINT_DEBUG) {
    Serial.print("Soll:");
    Serial.print(setpoint);
    Serial.print(",");
    Serial.print("Ist:");
    Serial.println(abs(actTorque));
  }
}