#include <PID_v1.h>
#include <Wire.h> // Library für I2C-Interface
#include <LiquidCrystal_I2C.h> // LCD i2c Library einbinden

LiquidCrystal_I2C lcd1(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);
LiquidCrystal_I2C lcd2(0x3F, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);


int HallPin = 2;
int pwmPin  = 9;
int PotiPin = 0;

//PID parameters
double Kp = 0, Ki = 10, Kd = 0;

double Setpoint, Input, Output;


int Umax = 1500; // Max Drehzahl des Motors
int upmSoll;
int Prozent; // Prozentwert aus Potistellung
int hpin;                       //  Impuls vom Hallgeber

float Force; // gemessene Kraft
unsigned long pwm = 0; //
char buf[17]; // Puffer für Zeichenkette
unsigned long upmIst = 0; // aktuell gemessene Drehzahl
int diff = 0; // Differenz zwischen Vorgabe und Ist-Drehzahl
//Define Variables we'll be connecting to
//create PID instance

const int AVG = 4; // Glättung über mindestens 4 Messwerte
volatile unsigned long dauer = 0; // microsekunden seit dem letzten Interrupt
volatile unsigned long last = 0; // Zählerwert beim letzten Interrupt
volatile unsigned long average = 1; // Integrierte Dauer // =1: divide by 0 vermeiden
volatile int avgcnt = 0;

PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

void setup()
{
  Serial.begin(9600);
  pinMode(PotiPin, INPUT); // lesender Zugriff
  pinMode(HallPin, INPUT); // dito
  pinMode(pwmPin, OUTPUT); // schreibender Zugriff
  analogWrite(pwmPin, 0); // für definierten Anfangszustand

  attachInterrupt(digitalPinToInterrupt(HallPin), readmicros, RISING);

  upmIst = analogRead(HallPin);
  upmSoll = 0;
  myPID.SetMode(AUTOMATIC);
  //Adjust PID values
  myPID.SetTunings(Kp, Ki, Kd);


  lcd1.begin(16, 2);
  lcd1.backlight();

  lcd2.begin(16, 2);
  lcd2.backlight();
}
void loop()
{

  Prozent = map(analogRead(PotiPin), 0, 1023, 0, 100);
  upmSoll = map(Prozent, 0, 100, 0, Umax);

  hpin = digitalRead(HallPin);
  //Serial.print("HallPin: ");
  //Serial.println(hpin);




  myPID.Compute();
  //analogWrite(pwmPin, Output); //LED is set to digital 3 this is a pwm pin.
  Serial.print(upmSoll);
  Serial.print(" ");
  Serial.println(pwm);
  Serial.print(" ");
  Serial.println(upmSoll);

UpmIst();

if (upmIst > upmSoll) {
  diff = upmIst - upmSoll;
  pwm = map(diff, 0, upmSoll, 0, 255);
  pwm = min(pwm, 255);
} else {
  diff = 0;
  pwm = 0;

}
analogWrite(pwmPin, pwm);

//Serial.print("Prozent: "); Serial.print(Prozent);
//Serial.print(" UpmSoll: "); Serial.print(upmSoll);
//Serial.print(" UpmIst: "); Serial.print(upmIst);
//Serial.print(" Diff: "); Serial.print(diff);
//Serial.print(" PWM: "); Serial.println(pwm);

 Lcd1();
 Lcd2();

//delay(500);
}
void UpmIst (void) {
  if (dauer != 0) {
    upmIst = (60000000 / average); // Drehzahl ausrechnen und runden
  } else {
    upmIst = 0; // keine Messung? -> Stillstand
    avgcnt = 0;
  }
  sprintf(buf, "%4lu", upmIst);
  Serial.println("_UpmIst: " + String(buf));

  dauer >>= 10;                                   // Flag für Stillstand ( : 1024 )
  //delay(500);
}

void readmicros() { // Interrupt-Routine
  detachInterrupt(digitalPinToInterrupt(HallPin)); // Interrupt ausschalten damit er uns nicht beißt
  int avgmax;
  unsigned long us = micros(); // Microsekundenzähler auslesen
  if (last == 0) { // erster Messwert?
    last = us; // merken und nicht weiter bearbeiten
  } else {
    if ( us < last ) { // Zählerüberlauf
      dauer = 4294967295 - last + us; // erzeugt einen Fehler von 1µS - vernachlässigbar
    } else {
      dauer = us - last; // Differenz zum letzten Durchlauf berechnen
    }
    if (dauer > 5000) { // ignorieren wenn <= 5ms (Kontaktpreller)
      average = dauer + average * avgcnt++; // Wert in buffer und mit Faktor avgcnt glätten
      average /= avgcnt; // und zurückrechnen
      avgmax = 1000000 / dauer; // dynamische Größe des Integrationspuffers
      if (avgmax < AVG) avgmax = AVG; // Trägheit mindestens 1 Sekunde
      if (avgcnt >= avgmax) avgcnt--;
      last = us; // und wieder den letzten Wert merken
  }
    else {
      dauer = 0;
    }
  }
  attachInterrupt(digitalPinToInterrupt(HallPin), readmicros, RISING ); // Interrupt wieder einschalten
}
void Lcd1(void) { // Ausgabe-Komplett am LCD1

  lcd1.setCursor(0, 0);
  lcd1.print("Force[Nm]:");
  lcd1.setCursor(10, 0);
  lcd1.print(Force, 3); // gewicht ausgabe
  lcd1.setCursor(0, 1);
  lcd1.print("Drehzahl ");
  lcd1.print(Prozent); // Prozent ausgabe
  lcd1.print("%");
}
void Lcd2(void) { // Ausgabe-Komplett am LCD2

  lcd2.clear(); // Soll es BLINKEN ODER NICHT????
  lcd2.setCursor(0, 0);
  lcd2.print("SOLL[U/min]:");
  lcd2.setCursor(12, 0);
  lcd2.print((int)upmSoll);
  lcd2.setCursor(0, 1);
  lcd2.print("IST [U/min]:");
  lcd2.setCursor(12, 1);
  lcd2.print((int)upmIst);
  // lcd2.setCursor(12, 1);
  // lcd2.print(Umax); //Max UPM Schwellwert zum schalten der Bremse
}