#define encoderPinA 22  // Pin für das Signal A
#define encoderPinB 23  // Pin für das Signal B

volatile int encoderPos = 0;  // Variable zur Speicherung der relativen Position
int lastEncoded = 0;          // Variable zur Speicherung des letzten Encodierten Zustands

int SerIn = 0;
float torque = 0;
float offset = 0;
float setpoint = 0;
unsigned int valA1 = 0;
unsigned int valA0 = 0;

void setup() {
  // Initialize the DAC pin
  analogWrite(DAC0, 0);
  pinMode(DAC0, OUTPUT);
  analogWrite(DAC1, 0);
  pinMode(DAC1, OUTPUT);

  pinMode(A0, INPUT);
  pinMode(A1, INPUT);

  pinMode(encoderPinA, INPUT);
  pinMode(encoderPinB, INPUT);
  attachInterrupt(digitalPinToInterrupt(encoderPinA), updateEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderPinB), updateEncoder, CHANGE);

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

  Serial.begin(9600);

  //delay(10000); // Evtl. vorhandene Spulenspannung senken.
}

void loop() {
  if (Serial.available() > 0) {
    delay(100);
    SerIn = Serial.read();
    if (SerIn == 't') {
      tare();
    } else {
      SerIn = (SerIn - '0') * 10;
      analogWrite(DAC1, SerIn * 40.95);
      Serial.println(SerIn * 0.26);
    }
  }



  torque = getActTorque();
  for (int i = 0; i < 9; i++) {
    torque += getActTorque();
  }
  torque = torque * 0.1;

  Serial.print("torque: ");
  Serial.println(abs(torque + offset), 1);
  analogWrite(DAC0, abs(torque + offset) * 81.9);



  // Beispiel: Ausgabe der relativen Position
  Serial.print("Relative Position: ");
  Serial.println(encoderPos);

  // Berechnung des relativen Winkels
  float angle = (encoderPos / 20000.0) * 360.0;
  Serial.print("Relative Angle: ");
  Serial.println(angle, 1);  // Ausgabe mit einer Nachkommastelle

  delay(1000);
}

void updateEncoder() {
  int MSB = digitalRead(encoderPinA);  // Most Significant Bit
  int LSB = digitalRead(encoderPinB);  // Least Significant Bit

  int encoded = (MSB << 1) | LSB;          // Kombinieren der beiden Bits
  int sum = (lastEncoded << 2) | encoded;  // Kombinieren mit dem letzten Zustand

  // Bestimmen der Richtungsänderung und Aktualisierung der Position
  if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) encoderPos++;
  if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) encoderPos--;

  lastEncoded = encoded;  // Aktualisieren des letzten Zustands
}

unsigned int VoltToInt(unsigned int voltage) {
  return (voltage - 550.0) * 1.86136364;  // (x - 550) * 4095/2200
}

float getActTorque() {
  valA0 = analogRead(A0);
  return (valA0 - 2047.5) * 0.024420024;  // (x - 2047.5) * 100/4095
  //return ((4095 * 1.00) - 2047.5) * 0.024420024;
}

float getSetPoint() {
  return (analogRead(A1) * 0.002442002);  //x * 10/4095
}

void tare() {
  torque = getActTorque();
  for (int i = 0; i < 99999; i++) {
    torque += getActTorque();
  }
  torque = torque * 0.00001;
  offset = 0 - torque;

  encoderPos = 0;
}