#include <Wire.h>               // Für die I2C-Kommunikation
#include <Adafruit_MCP4725.h>   // Die Library für den DAC
#include <Servo.h>


float zwischenErstUndZweitPulsTemp = 0.0;
float zwischenErstUndZweitPulsTest = 1500;
float zwischenErstUndZweitPuls = 0;
int endSpeed2 = 0;
float stabilerWert = 0;
int kmh = 0;


unsigned long firstPuls = 0.0;
unsigned long firstPulsMerker = 0.0;

bool PedalPuls = true;
bool PedalPulsMerker = true;
bool pedal = false;
// bool timerstartRad = false;
int timerzaehler = 0;
int pedalTimerzaehler = 0;
int mainZaehler = 0;
int softwareTester = 0;
int pedalPulsZahl = 0;
// int startGrenzeRad = 200; // StartLimit more than 1 puls in one secont
int radLaeftZaehler = 0;
int pulsRadZaehler = 0;
int radLaeft = 0;
bool radAmLaufen = false;
// int stufen[] = {655, 1212, 1769, 2326, 2883, 3441};
int stufenWerte[] = {0, 246, 492, 738, 984, 1211};
int gradStufen[] = {0, 20, 50, 90, 140, 180};

Servo arm; // Create a "Servo" object called "arm"
int pos = 0; // Variable where the arm's position will be stored (in degrees)
int voltag = 0; // Variable where the arm's position will be stored (in degrees)


volatile bool wurdeAusgefuehrt = false;
float unterschied = 0.0;

Adafruit_MCP4725 dac;


void setup() {
  Serial.begin(9600);
  pinMode(3, INPUT_PULLUP);
  pinMode(13, OUTPUT);

  arm.attach(4); // Attache the arm to the pin 2
  arm.write(pos);

  /*
  if (!dac.begin(0x62)) {
    Serial.println("MCP4725 nicht gefunden!");
    digitalWrite(13, HIGH);
    while (1);
  }
  digitalWrite(13, LOW);
  Serial.println("MCP4725 bereit.");
  // put your setup code here, to run once:
  dac.setVoltage(655, false);
  */
  pinMode(2, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(2), pulszaehler, FALLING);
}

void loop() {

  PedalPuls = digitalRead(3);

  if (wurdeAusgefuehrt == true) {
    firstPuls = millis();
    radLaeftZaehler = 0;
    radLaeft++;
    zwischenErstUndZweitPulsTest = firstPuls - firstPulsMerker;
    firstPulsMerker = firstPuls;
    wurdeAusgefuehrt = false;
  }

  if (radLaeftZaehler < 150) {
    if (radLaeft > 1) {
      radAmLaufen = true;
      if (radLaeft < 3) {
        zwischenErstUndZweitPulsTest = 1200;
        // Serial.println(zwischenErstUndZweitPulsTest);
      }
      if (radLaeft > 1000) {
        radLaeft = 10;
      }
    } else {
      radAmLaufen = false;
      // zwischenErstUndZweitPulsTest = 1500;
      // firstPulsMerker = firstPuls + 1500;
    }
    radLaeftZaehler++;
  } else {
    zwischenErstUndZweitPulsTest = 1500;
    // firstPulsMerker = firstPuls;
    radAmLaufen = false;
    pulsRadZaehler = 0;
    radLaeft = 0;
  }


  if (PedalPuls == false && PedalPulsMerker == true) {
    pedalTimerzaehler = 0;
    pedalPulsZahl++;
  }
  PedalPulsMerker = PedalPuls;


  if (pedalTimerzaehler < 100) {
    if (pedalPulsZahl > 2) {
      pedal = true;
      if (pedalPulsZahl > 1000) {
        pedalPulsZahl = 10;
      }
    } else {
      pedal = false;
    }
    pedalTimerzaehler++;
  } else {
    pedalPulsZahl = 0;
    pedal = false;
  }

  // To Improve *******************************************************
  if (mainZaehler > 10) {
    zwischenErstUndZweitPulsTest = constrain(zwischenErstUndZweitPulsTest, 280, 1500);

    zwischenErstUndZweitPulsTemp = map(zwischenErstUndZweitPulsTest,280, 1500, 1220, 0);

    if (abs(zwischenErstUndZweitPulsTemp - stabilerWert) > 100) {
      stabilerWert = zwischenErstUndZweitPulsTemp; // Nur dann übernehmen wir den neuen Wert
    } else if (zwischenErstUndZweitPulsTemp == 0) {
      stabilerWert = 0; // Nur dann übernehmen wir den neuen Wert
    }

    if (pedal == true && radAmLaufen == true) {

      unterschied = stabilerWert - endSpeed2;
      endSpeed2 = endSpeed2 + unterschied * 0.1;

    } else {
      if (endSpeed2 > 100) {
        endSpeed2 = endSpeed2 - 100;
      } else {
        endSpeed2 = endSpeed2 - 10;
      }
      if (endSpeed2 < 0) {
          endSpeed2 = 0;
      }
    }
    endSpeed2 = constrain(endSpeed2, 0, 1211);

    
    

    voltag = map(endSpeed2, 0, 1221, 655, 3441); // 655 bis 3441
    pos = map(endSpeed2, 0, 1211, 0, 180);
    kmh = map(endSpeed2, 0, 1211, 0, 25);

    Serial.println(String(zwischenErstUndZweitPulsTemp) + " >>>> " + String(stabilerWert) + " >>>> " + String(endSpeed2) + " >>>> " + String(pos) + " >>>> " + String(voltag) + " >>>> " + String(kmh));

    arm.write(pos); // Set the arm's position to "pos" value


    mainZaehler = 0;
    
  }

  // softwareTester++;
  mainZaehler++;
  delay(10);
}


void pulszaehler () {
  if (!wurdeAusgefuehrt) {
    // Serial.println("test");
    wurdeAusgefuehrt = true;
  }
}