#include <prg_342.h>

PRG_342 prg = PRG_342();

int angle = 0;
int lastAngle = 0;
int actTorque = 0;
int setpoint = 0;
double lastOutput;

float Kp = 0.0;
float Ki = 0.0;
float Kd = 0.0;
int Pout;
int Iout;
int Dout;

int error;

bool OFFSET_ON = true;
bool manOut = false;

bool PID = false;
unsigned char PID_DELAY = 1;  //in ms
unsigned int Imax = 217;      //in mA
bool PRINT_DEBUG = false;
bool DEBUG_MODE = false;

// Define variables for PID calculations
float integral = 0;
int derivative;
int previousError = 0;
unsigned long lastTime = 0;
unsigned long currentTime;
unsigned long timeChange;

unsigned long timer1;
unsigned long timer2;
unsigned long PIDtimer;

float RotSpeed;
unsigned int MilliAmpPerSecond = 160;
unsigned int maxChange;

char serBuffer[4];

void setup() {
  analogWriteResolution(12);  // Set the resolution to 12 bits (0-4095)
  analogReadResolution(12);

  Serial.begin(115200);

  prg.safeShutdown(5);

  timer1 = timer2 = PIDtimer = millis();
  angle = prg.getAngle();
  lastAngle = angle;
}

void COMMUNICATION_HANDLER() {
  if (Serial.available() > 0) {
    delay(10);
    unsigned int SerIn = Serial.read();
    switch (SerIn) {
      case 'a':
        Serial.print((float)prg.getAngle() / 1000, 1);
        Serial.print(";");
        Serial.println((float)prg.getTorque(false, 10) / 1000, 1);
        break;

      case 'e':
        prg.safeShutdown(10);
        break;

      case 't':
        prg.tareTorque();
        prg.tareAngle();
        break;

      case 'o':
        OFFSET_ON = !OFFSET_ON;
        break;

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

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

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

      case 's':
        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':
        manOut = !manOut;
        break;

      case 'v':
        if (manOut) {
          Serial.readBytes(serBuffer, 4);
          unsigned int volt = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001;
          prg.setOutput(40.95 * volt, false);
        }
        break;

      case 'x':
        PID = !PID;
        error = 0;
        previousError = 0;
        integral = 0;
        break;

      case 'r':
        error = 0;
        previousError = 0;
        integral = 0;
        derivative = 0;
        Pout = 0;
        Iout = 0;
        Dout = 0;
        break;

      case 'f':
        prg.DynDecog(prg.ActOut);
        break;

      case 'k':
        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 = !DEBUG_MODE;
        break;

      default:
        Serial.println(SerIn);
        break;
    }
  }
}

void PID_CONTROL() {
  // Get current time
  currentTime = millis();
  timeChange = currentTime - lastTime;
  lastTime = currentTime;

  // Read the current value from the sensor
  actTorque = abs(prg.getTorque(true, 10));
  angle = abs(prg.getAngle());

  // Calculate error
  error = setpoint - actTorque;

  // Proportional term
  Pout = Kp * error;

  // Integral term
  integral += error * (float)timeChange / 1000.0;  // Convert timeChange to seconds
  // Limit the integral term to prevent windup
  integral = constrain(integral, -100000, 100000);
  Iout = Ki * integral;

  // Derivative term
  derivative = (error - previousError) / ((float)timeChange / 1000.0);  // Convert timeChange to seconds
  Dout = Kd * derivative;

  // Calculate total output
  int output = Pout + Iout + Dout;

  // Constrain output
  output = constrain(output, 10, 4095);

  // Implement a smooth change to avoid sudden jumps
  static int lastOutput = 0;
  //int maxChange = 10;  // Max change per loop iteration (adjust as needed)
  maxChange = (MilliAmpPerSecond * 4095 * PID_DELAY) / (Imax * 1000);
  if (abs(output - lastOutput) > maxChange) {
    if (output > lastOutput) {
      output = lastOutput + maxChange;
    } else {
      output = lastOutput - maxChange;
    }
  }

  // Write the PID output to the actuator
  prg.setOutput(output, false);
  lastOutput = output;

  // Update previous error
  previousError = error;
}

void SPEEDCHECK() {
  if (millis() - timer2 >= 200) {
    timer2 = millis();
    int deltaAngle = prg.getAngle() - lastAngle;
    RotSpeed = deltaAngle * 5.0;  // Convert to speed per second
    lastAngle = prg.getAngle();
  }
}

void DEBUG_TIMER() {
  if (millis() - timer1 >= 500) {
    timer1 = millis();
    PRINT_DEBUG = true;
  } else {
    PRINT_DEBUG = false;
  }
}

void DEBUG_PRINTER() {
  if (PRINT_DEBUG) {
    Serial.println("####################");
    Serial.print("Winkel: ");
    Serial.println((float)prg.getAngle() / 1000, 3);
    Serial.print("Drehmoment: ");
    Serial.println((float)prg.getTorque() / 1000, 3);
    Serial.print("Geschwindigkeit: ");
    Serial.println(RotSpeed);
    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("Pout: ");
    Serial.println(Pout);
    Serial.print("Iout: ");
    Serial.println(Iout);
    Serial.print("Dout: ");
    Serial.println(Dout);
    Serial.print("Integral: ");
    Serial.println(integral);
    Serial.print("PID: ");
    Serial.println(PID);
    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.println("####################");
  }
}

void loop() {
  if (DEBUG_MODE) {
    DEBUG_TIMER();
    DEBUG_PRINTER();
  }
  COMMUNICATION_HANDLER();
  SPEEDCHECK();

  if (millis() - PIDtimer >= PID_DELAY) {  // Small delay for stability
    PIDtimer = millis();

    if ((PID && !manOut && abs(prg.getTorque()) >= 2000 && abs(RotSpeed) >= 400) || (abs(prg.getTorque()) > setpoint)) {
      PID_CONTROL();
    }
  }

  prg.updateActOut();
  prg.isRunning = false;
}