//#include <prg_342.h>
#include <Libraries/PRG_342/prg_342.h>
#include <Libraries/datalogger/Datalogger.h>
#include <Libraries/pid/pid.h>
#include <string>
#include <math.h>

#define DEBUG_PRINT_TIME 500
#define SPEEDCHECK_TIME 200
#define PID_DELAY 0
#define LOG_TIMER_DELAY 100
#define MEAS_DELAY 0
#define K_FAKTOR 1.0995380532183
#define OFFSET 0
#define ALPHA 0.0008
#define N_SAMPLES 100
#define BLOCK_DETECTION_VALUE setpoint
#define ROTATION_DETECTION_VALUE 100

#define _getTorque abs(prg.getTorque(false, ALPHA))
#define _getAngle prg.getAngle()

struct Timer {
  unsigned long startMillis;
  unsigned long interval;
  bool active;
};

void waitStart(Timer &timer, unsigned long interval) {
  timer.startMillis = millis();
  timer.interval = interval;
  timer.active = true;
}

bool waitIsOver(Timer &timer) {
  if (timer.active && (millis() - timer.startMillis >= timer.interval)) {
    timer.active = false;
    return true;
  }
  return false;
}

Timer debug_timer;
Timer pid_timer;
Timer speedcheck_timer;
Timer log_timer;
Timer meas_timer;

PRG_342 prg = PRG_342(K_FAKTOR);
Datalogger datalogger = Datalogger();
PID myPID(0.0, 0.0, 0.0);

//Messung / Logging
bool LOGGING = false;

// Deklaration der Variablen für die Steuerung und Messung
int angle;
int lastAnglePID;
int lastAngle;
int actTorque;
//int actTorqueArray[100];
float filteredTorque = 0;  // gefilterter Wert
float filteredSetpoint = 0;
float setpoint;
int oldSetpoint;
static int lastOutput;
float manVolt;
unsigned char CW;
unsigned char Richtungswechsel;
float RotSpeed;
unsigned char BLOCK;
unsigned char BLOCK_SOLVED;

bool In_Src_Sw = false;

float Kp = 0.0;
float Ki = 0.1;
float Kd = 0.0;

/*float Kp = 1.0;
float Ki = 0.333;
float Kd = 1.0;*/

float WinkelArray[181];

// Steuervariablen
bool manOut = false;      // Manuelle Ausgabe aktivieren/deaktivieren
bool Pon = true;          // PID-Regelung ein-/ausschalten
bool DEBUG_MODE = false;  // Debug-Modus ein-/ausschalten

// Initialisierung der verschiedenen Timer
unsigned long logtime;

void setup() {
  setAnalogResolution(12);


  waitStart(debug_timer, DEBUG_PRINT_TIME);
  waitStart(pid_timer, PID_DELAY);
  waitStart(speedcheck_timer, SPEEDCHECK_TIME);
  waitStart(log_timer, LOG_TIMER_DELAY);
  waitStart(meas_timer, MEAS_DELAY);

  myPID.setOutputLimits(0, 4095);
  myPID.setSampleTime(PID_DELAY);
  myPID.setTunings(Kp, Ki, Kd);

  //prg.safeShutdown(3);

  UPDATE_VALUES();
  /*for (int i = 0; i < N_SAMPLES; i++) {
    actTorqueArray[i] = actTorque;
  }*/
  filteredTorque = actTorque;

  lastAngle = lastAnglePID = angle;

  for (int i = 0; i < 181; i++) {
    WinkelArray[i] = -1;
  }

  Serial.begin(19200);  // Beginne serielle Kommunikation
}

void loop() {
  /*if (waitIsOver(meas_timer)) {
    UPDATE_VALUES();
    for (int i = 0; i < N_SAMPLES - 1; i++) {
      actTorqueArray[i] = actTorqueArray[i + 1];
    }
    actTorqueArray[N_SAMPLES - 1] = actTorque;
    actTorque = 0;
    for (int j = 0; j < N_SAMPLES; j++) {
      actTorque += actTorqueArray[j];
    }
    actTorque = (float)actTorque / N_SAMPLES;
    waitStart(meas_timer, MEAS_DELAY);
  }*/
  UPDATE_VALUES();
  filteredTorque = exponentialFilter(actTorque, filteredTorque, ALPHA);  // Verwenden Sie einen geeigneten Alpha-Wert

  COMMUNICATION_HANDLER();

  if (DEBUG_MODE) {
    if (waitIsOver(debug_timer)) {
      DEBUG_PRINTER();
      waitStart(debug_timer, DEBUG_PRINT_TIME);
    }
  }

  if (waitIsOver(speedcheck_timer)) {
    SPEEDCHECK();

    BLOCK = BLOCK_DETECTION(abs(actTorque), abs(RotSpeed));

    /*if (BLOCK == 0) {
      if (BLOCK_SOLVED == 1) {
        setpoint = oldSetpoint;
        BLOCK_SOLVED = 0;
      } else {
        oldSetpoint = setpoint;
      }
    } else if (BLOCK == 1) {
      if (BLOCK_SOLVED == 0) {
        setpoint = 0;
        BLOCK_SOLVED = 1;
      }
    }*/

    waitStart(speedcheck_timer, SPEEDCHECK_TIME);
  }


  if (Richtungswechsel) {
    myPID.reset();
    Richtungswechsel = 0;
  }

  if (!BLOCK) {
    // Aktualisieren des Setpoints basierend auf dem Winkel
    int index = (float)angle * 0.002;  // 0.5 Grad pro Index
    if (index >= 0 && index < 181) {
      setpoint = WinkelArray[index];
    }
    if (In_Src_Sw) {
      setpoint = float(prg.getSetPoint(1)) * 5.0;
    }
  }

  /*if (abs(setpoint) - abs(actTorque) <= 1000) {
    myPID.setTunings(Kp, Ki, Kd);
  } else {
    myPID.setTunings(Kp, Ki, Kd);
  }*/
  myPID.setTunings(Kp, Ki, Kd);

  if (lastAnglePID != angle && Pon && !manOut && ((abs(actTorque) >= 1000 || abs(RotSpeed) >= 400) || abs(actTorque) > setpoint)) {
    unsigned int output = myPID.compute(setpoint, actTorque);
    prg.setOutput(output);
    prg.ActOut = output;
  } else if (abs(actTorque) < 1000) {
    myPID.reset();
  }
  lastAnglePID = angle;

  if (LOGGING) {
    if (waitIsOver(log_timer)) {
      sendDataToDatalogger();
      waitStart(log_timer, LOG_TIMER_DELAY);
    }
  }
  prg.isRunning == false;
}

void interpolateWinkelArray() {
  int lastSetIndex = -1;

  for (int i = 0; i < 180; i++) {
    int start = i;
    int k = i + 1;
    while (WinkelArray[k] == -1) {
      k++;
    }
    int end = k;
    float startTorque = WinkelArray[start];
    float endTorque = WinkelArray[end];

    for (int j = start + 1; j < end; j++) {
      float interpolatedTorque = startTorque + ((endTorque - startTorque) / (end - start)) * (j - start);
      WinkelArray[j] = interpolatedTorque;
    }
  }

  /*for (int i = 0; i < 181; i++) {
    if (WinkelArray[i] != 0 || i == 0) {  // Sicherstellen, dass der Startwert berücksichtigt wird
      if (lastSetIndex != -1) {
        int start = lastSetIndex;
        int end = i;
        float startTorque = WinkelArray[start];
        float endTorque = WinkelArray[end];

        for (int j = start + 1; j < end; j++) {
          float interpolatedTorque = startTorque + ((endTorque - startTorque) / (end - start)) * (j - start);
          WinkelArray[j] = interpolatedTorque;
        }
      }
      lastSetIndex = i;
    }
  }*/
}



float exponentialFilter(float currentValue, float previousFilteredValue, float alpha) {
  return alpha * currentValue + (1.0 - alpha) * previousFilteredValue;
}

void sendDataToDatalogger() {
  datalogger.addData((float)((float)millis() - (float)logtime) / 1000.0, (float)angle / 1000.0, setpoint / 1000.0, actTorque / 1000.0, (float)prg.getOutVolt() / 1000.0, RotSpeed);
}

unsigned char BLOCK_DETECTION(unsigned int torque, float speed) {
  if (torque >= BLOCK_DETECTION_VALUE && speed < ROTATION_DETECTION_VALUE) {
    return 1;
  } else {
    return 0;
  }
}

void setAnalogResolution(unsigned char res) {
  analogWriteResolution(res);
  analogReadResolution(res);
}

void UPDATE_VALUES() {
  angle = _getAngle;
  actTorque = _getTorque;

  prg.updateActOut(actTorque);
}

void COMMUNICATION_HANDLER() {
  if (Serial.available() > 0) {
    String command = receiveString();
    parseCommand(command);
    Serial.read();
  }
}

void SPEEDCHECK() {
  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_PRINTER() {
  // Debug-Informationen ausgeben
  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("Kp: ");
  Serial.println(Kp, 3);
  Serial.print("Ki: ");
  Serial.println(Ki, 3);
  Serial.print("Kd: ");
  Serial.println(Kd, 3);
  Serial.print("output: ");
  Serial.println((unsigned int)((float)prg.getOutVolt() * 0.1575));
  Serial.print("Pon: ");
  Serial.println(Pon);
  Serial.print("manOut: ");
  Serial.println(manOut);
  Serial.print("manVolt: ");
  Serial.println(manVolt);
  Serial.print("LOGGING: ");
  Serial.println(LOGGING);
  Serial.println("####################");
}


String receiveString() {
  String received = "";
  unsigned long startTime = millis();
  byte inByte[1];
  while ((char)inByte[0] != '\n' || Serial.available() > 0) {
    //c = Serial.read();
    Serial.readBytes(inByte, 1);
    received += (char)inByte[0];
  }
  //Serial.println(received);
  return received;
}

void parseCommand(const String &command) {
  if (command[0] == 'p') {
    float tempKp;
    setParameter(command, 'Kp', tempKp);
    Kp = tempKp;
    myPID.setTunings(Kp, Ki, Kd);            // Aktualisieren der Tunings
    datalogger.addConfig("Kp", String(Kp));  // Konvertierung zu String
  } else if (command[0] == 'i') {
    float tempKi;
    setParameter(command, 'Ki', tempKi);
    Ki = tempKi;
    myPID.setTunings(Kp, Ki, Kd);            // Aktualisieren der Tunings
    datalogger.addConfig("Ki", String(Ki));  // Konvertierung zu String
  } else if (command[0] == 'd') {
    float tempKd;
    setParameter(command, 'Kd', tempKd);
    Kd = tempKd;
    myPID.setTunings(Kp, Ki, Kd);            // Aktualisieren der Tunings
    datalogger.addConfig("Kd", String(Kd));  // Konvertierung zu String
  } else if (command[0] == 'a') {
    Serial.print(angle);
    Serial.print(";");
    Serial.print(actTorque);
    Serial.print(";");
    Serial.print(prg.getSetPoint(1));
    Serial.print(";");
    Serial.print(setpoint);
    Serial.print(";");
    Serial.println(prg.ActOut);
  } else if (command[0] == 'b') {
    DEBUG_MODE = !DEBUG_MODE;
  } else if (command[0] == 'e') {
    prg.safeShutdown(5);
  }
  if (command[0] == 'f') {
    prg.DynDecog(prg.ActOut);
  } else if (command[0] == 'l') {
    LOGGING = !LOGGING;
    if (LOGGING) {
      if (CW) {
        datalogger.addConfig("Drehrichtung", "CW");
      } else {
        datalogger.addConfig("Drehrichtung", "CCW");
      }
      logtime = millis();
      waitStart(log_timer, LOG_TIMER_DELAY);
    } else {
      datalogger.logToSerial();
      datalogger.clear();
    }
  } else if (command[0] == 'm') {
    manOut = !manOut;
  } else if (command[0] == 'r') {
    myPID.reset();
  } else if (command[0] == 's') {
    setParameter(command, 's', setpoint);
    for (int i = 0; i < 180; i++) {
      WinkelArray[i] = setpoint;
    }
  } else if (command[0] == 'S') {
    In_Src_Sw = !In_Src_Sw;
  } else if (command[0] == 't') {
    prg.tareTorque();
  } else if (command.startsWith("u") && command.endsWith("u\n")) {
    Serial.println(command);  // Debug-Ausgabe
    for (int i = 0; i < 181; i++) {
      WinkelArray[i] = -1;
    }
    String data = command.substring(1, command.length() - 1);
    int lastIndex = 0;
    while (lastIndex != -1) {
      int index = data.indexOf(';', lastIndex);
      String pair = (index == -1) ? data.substring(lastIndex) : data.substring(lastIndex, index);
      int commaIndex = pair.indexOf(',');
      if (commaIndex != -1) {
        int angleIndex = pair.substring(0, commaIndex).toInt();
        float torqueValue = pair.substring(commaIndex + 1).toFloat();
        if (angleIndex >= 0 && angleIndex < 181) {
          WinkelArray[angleIndex] = torqueValue;
          Serial.print("Set WinkelArray[");
          Serial.print(angleIndex);
          Serial.print("] to ");
          Serial.println(torqueValue);
        }
      }
      lastIndex = (index == -1) ? -1 : index + 1;
    }
    interpolateWinkelArray();
  } else if (command[0] == 'v') {
    if (manOut) {
      setParameter(command, 'v', manVolt);  // Hier wurde 'manVolt' als float deklariert
      manVolt = manVolt * 0.1575;
      prg.ActOut = manVolt;
      analogWrite(prg.TORQUE_OUT_PIN, manVolt);
    }
  } else if (command[0] == 'w') {
    prg.tareAngle();
  } else if (command[0] == 'x') {
    Pon = !Pon;
    myPID.reset();
  }
}


void setParameter(const String &command, char prefix, float &parameter) {
  int commaIndex = command.indexOf('.');
  if (commaIndex > 1) {
    String paramString = command.substring(1, commaIndex) + "." + command.substring(commaIndex + 1);
    parameter = paramString.toFloat();  // Konvertierung innerhalb der Funktion
    /*Serial.print(prefix);
    Serial.print(" gesetzt auf: ");
    Serial.println(parameter, 6);  // Sechs Dezimalstellen anzeigen*/
  }
}