// verwendete Bibliotheken
#include <Wire.h>;
#include <LCD.h>;
#include <LiquidCrystal_I2C.h>;
#include <EEPROM.h>;
#include <Servo.h>
#include <LedControl.h>
#include <IRremote.h>
#include <Stepper.h>

// definierte Konstanten
#define prell     2   // Prellzeit [ms]
#define laenge    190 // Abstand der Lichtschranken [mm]
#define anzeigezeit 3000 // 3 sekunden Standzeit der Anzeige

//Drehwähler
#define pin_clk   0   // Drehgeber CLK Violett
#define pin_dre   1   // Drehgeber DRE Grau
#define pin_but   2   // Zentralknopf SW Gelb

//Auslöser
#define pin_irs   9   // IR Auslösesendediode
#define als       0xB4B8F  // Auslösekommando

//Servo
#define pin_sst   4   // Revolverabzug; Strom
#define pin_spo   5   // Signale für servo
#define se0       0   // Positionen
#define se1       180
#define sew       2000 // Setzzeit  [ms]

//Lichtschranke
#define pin_ls0   6   // pin der Meßlichtschranke 0
#define pin_ls1   7   // pin der Meßlichtschranke 1

//Blitz
#define pin_bli   8   // Blitzauslöserpin, 2 Kabel schwarz+gelb
#define laz       6000 //  und Ladezeit[ms]

//Starttaster
#define pin_wei   10    // Taster 'weiter' gelbes Kabel
#define pin_wli   3     // Licht für Weiter-Taster grünes Kabel (war 9; Tausch mit IR-Pin 3-Nano auf 9-MEGA)
#define pin_prw   30    // Kippschalter Vorlauf / Hauptlauf grünes Kabel

//Einblendeanzeige
#define lda       2000 //  [ms]
#define pin_ldi   11 // data in
#define pin_lcs   13 // CS
#define pin_lcl   12 // clock

//Schrittmotor
#define pin_sla   32    /*  Workaraound, sollte die Direktansteuerung nicht klappen für Mikro-Arduino
                            der dann die Ansteuerung des SM übernimmt
                            derzeit nicht belegt
                            */
#define pin_sm0   22    // Vierfachkabel zur H-Brücke (2 Stück); SM Strom getrennt, gelb
#define pin_sm1   24    // lilagrau
#define pin_sm2   26    // orange
#define pin_sm3   28    // lila‚
#define schritteProDrehung 2048
#define drehgeschwindigkeit 5 // rpm

//Anzeigen
#define lcd_aktiv true
#define prn_aktiv true
#define debug     false

// Initialisierung der Bibliotheken
LedControl        lc =  LedControl(pin_ldi, pin_lcs, pin_lcl, 1);
IRsend            kamera;
Servo             abzug;
Stepper           Drehscheibe(schritteProDrehung, pin_sm0, pin_sm1, pin_sm2, pin_sm3);
LiquidCrystal_I2C lcd (0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);

// Variablendefinition

char *Variable[]  = {
  // Label für Parametereinstellung
  "n Kugeln   : ",
  "mm Distanz : ",
  "t0 1.Foto  : ",
  "t Inkrement: ",
  // Label für Bildschleife
  "Kugel Nr.  : ",
  "V0 (dm/s)  : ",
  "tn-t0 (ms) : ",
  "delta s(mm): ",
};

int adresse = EEPROM.read(0) + 256 * EEPROM.read(1);

int Wert[] = {
  (int)EEPROM.read(adresse * 8 + 2) + 256 * (int)EEPROM.read(adresse * 8 + 3),
  (int)EEPROM.read(adresse * 8 + 4) + 256 * (int)EEPROM.read(adresse * 8 + 5),
  (int)EEPROM.read(adresse * 8 + 6) + 256 * (int)EEPROM.read(adresse * 8 + 7),
  (int)EEPROM.read(adresse * 8 + 8) + 256 * (int)EEPROM.read(adresse * 8 + 9),
  5, 6, 7, 8
};

int Min[] = {  1,  200,  -200,  0}; // n Kugel, Flugstrecke [mm], erster Auslösezeitpunkt  [us], Intervall  [us]
int Max[] = { 6, 2000,  500,  500};
int Inkrement[] = {  1,  5,  10,  5};

// Ablauf Variablen
const unsigned long len = laenge; // Länge der Lichtschranke [mm]
int aender = 0; // index des aktuell zu ändernden Parameters [0,3]

// Funktionsdefinitionen

void speichern() {
  EEPROM.write(adresse * 8 + 2, lowByte(Wert[0]));    EEPROM.write(adresse * 8 + 3, highByte(Wert[0]));
  EEPROM.write(adresse * 8 + 4, lowByte(Wert[1]));    EEPROM.write(adresse * 8 + 5, highByte(Wert[1]));
  EEPROM.write(adresse * 8 + 6, lowByte(Wert[2]));    EEPROM.write(adresse * 8 + 7, highByte(Wert[2]));
  EEPROM.write(adresse * 8 + 8, lowByte(Wert[3]));    EEPROM.write(adresse * 8 + 9, highByte(Wert[3]));
};

void Anzeige(bool lc, bool sp, bool pt) {

  int ptm = 0;
  if (pt) {
    ptm = 4;
  };

  if (sp) {
    Serial.println();
    for (int i = 0 + ptm; i < 4 + ptm; i++) {
      Serial.print(Variable[i]);
      Serial.print(Wert[i]);

      if ((!pt) and (aender == i)) {
        Serial.print("**");
      };
      Serial.println();
    };
  };

  if (lc) {
    lcd.clear();
    lcd.home();
  }
  for (int i = 0; i < 4; i++) {
    lcd.setCursor(0, i);
    lcd.print(Variable[i + ptm]);
    lcd.setCursor(13, i);
    lcd.print(Wert[i + ptm]);
    if ((aender == i) and (!pt)) {
      lcd.setCursor( 18, i);
      lcd.print("**");
    };
  };
};

void starttaster() {
  digitalWrite(pin_wli, HIGH); // leuchtet!
  while (digitalRead(pin_wei) and sicherheit()) {}; // wartet auf das drücken und prüft, dass der Programmschalter auf 'Lauf' steht
  delay(prell * 10);
  digitalWrite(pin_wli, LOW); // leuchtet nicht mehr...
  ;
};

int drehung() {
  //  Frage Drehsignalgeber I ab
  if (digitalRead(pin_clk) == true) {
    return 0;
  };
  // keine Drehung detektiert

  delay(prell);

  if (digitalRead(pin_clk) == true) {
    return 0;
  };
  // est ein positives Signal (LOW) aber dann Drehung nicht bestätigt

  if (digitalRead(pin_dre) == true) {
    delay (prell);
    while (!(digitalRead(pin_dre) and digitalRead(pin_clk))) {
      delay(prell);
    };
    return -1;
  };
  // Drehung bestätigt im Sinn -1

  if (digitalRead(pin_dre) == false) {
    delay (prell);
    while (!(digitalRead(pin_dre) and digitalRead(pin_clk))) {
      delay(prell);
    };
    return 1;
  };
  // Drehung bestätigt im Sinn +1
};

void parameter() {
  // Konopf gedrückt um nächste Eingabeabfrage zu setzen
  if (!digitalRead(pin_but)) {
    delay(prell);
    if (!digitalRead(pin_but)) {
      if (debug) {
        Serial.println("Knopf gedrückt");
      };
      while (!digitalRead(pin_but)) {};
      delay(prell);
      if (aender < 3) {
        aender++;
      } else {
        aender = 0;
      };
      if (debug) {
        Serial.println("Knopf losgelassen");
      };
      Anzeige(lcd_aktiv, prn_aktiv, false);
    };
  };

  // Frage Drehgeber ab
  int drehpuls = drehung();
  if (drehpuls != 0) {
    Wert[aender] = Wert[aender] + drehpuls * Inkrement[aender];
    if (Wert[aender] < Min[aender]) {
      Wert[aender] = Min[aender];
    };
    if (Wert[aender] > Max[aender]) {
      Wert[aender] = Max[aender];
    };
    Anzeige(lcd_aktiv, prn_aktiv, false);
  };
};

// servo setzten; 0/1 Ruhe / Abzug und Stellzeit in ms warten
// Achtung: die Zeit ab Kommando '1" und bis Schuss ist niicht definiert, für Schuß daher Stellzeit 0!
void servo_setzen(int abzugpos, int stellzeit) {
  if (abzugpos == 0) {
    abzug.write(se0);
    delay(stellzeit);
  };
  if (abzugpos == 1) {
    abzug.write(se1);
    delay(stellzeit);
  };
  if (debug) {
    Serial.print("SP: ");
    Serial.print(abzugpos);
    Serial.print('\t');
  };
  return;
}

// Auslösung Kamera
void kamera_ausloesen() {
  for (int i = 0; i < 3; i++) {
    kamera.sendSony(als, 20); // Sony TV power code
    delay(50);
  };
};

// Blitzauslösung
void blitz_ausloesen (unsigned long ausloesezeitpunkt) {
  while (ausloesezeitpunkt >  micros()) {}; // wartet bis Auslösezeitpunkt eingetreten ist
  digitalWrite(pin_bli, LOW);
  delay(2);                // 2 ms
  digitalWrite(pin_bli, HIGH);
  return;
};

//  Bildschirme
void startbildschirm() {
  lcd.setBacklight(HIGH);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Vorbereitung:");
  lcd.setCursor(0, 1);
  lcd.print("=======");
  lcd.setCursor(0, 2);
  lcd.print("Eingabe");
  lcd.setCursor(0, 3);
  lcd.print("der Startwerte.");
  delay(anzeigezeit);
};

void zwischenbildschirm() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Es geht los!");
  Serial.print("Es geht los! ");
  delay(anzeigezeit);
  lcd.clear ();
};

void abschlussbildschirm() {
  lcd.setBacklight(HIGH);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Ende der Serie:");
  delay(anzeigezeit);
};

// Ausgabe 8-stellige 7-Sergment-LED
void led_setzen ( long versatz, float geschwindigkeit, int durchgang, int leuchtdauer)
{
  // Einzelziffern für Anzeige berechnen
  int v100 = 0;
  int v010 = 0;
  int v001 = 0;
  bool plu = false;
  int ve100 = 0;
  int ve010 = 0;
  int ve001 = 0;

  v100 = (int(geschwindigkeit) / 10) % 10;
  v010 = int(geschwindigkeit) % 10;
  v001 = int((geschwindigkeit * 10)) % 10;

  ve100 = abs(versatz) / 100 % 10;
  ve010 = abs(versatz) / 10 % 10;
  ve001 = abs(versatz) % 10;
  if (versatz < 0) {
    plu = false;
  } else {
    plu = true;
  };

  // LED Setzen
  // plus/Minus
  if (!plu) {
    lc.setChar(0, 7, "c", false);
  } else {
    lc.setChar(0, 7, " ", false);
  };

  // versatz
  lc.setDigit(0, 6, ve100, false);
  lc.setDigit(0, 5, ve010, false);
  lc.setDigit(0, 4, ve001, true);

  // durchgang
  lc.setDigit(0, 3, durchgang, true);

  // V0
  lc.setDigit(0, 2, v100, false);
  lc.setDigit(0, 1, v010, true);
  lc.setDigit(0, 0, v001, false);

  delay(leuchtdauer); // Zeitdauer in msec für Ziffern an
  lc.clearDisplay(0); // löschen

};

bool sicherheit() {
  if (digitalRead(pin_prw)) {
    return true;
  }
  else {
    return false;
  };
};

void zieldrehung () {
  Drehscheibe.step(schritteProDrehung / 6);
  digitalWrite(pin_sla, LOW);
  delay(20);
  digitalWrite(pin_sla, HIGH);
};

void schritt() {
  while (digitalRead(pin_wei)) {};
  delay(50);
};

void bildlauf(int f_kug, int f_dis, int f_ve0, int f_dti, unsigned long f_len ) {
  digitalWrite(pin_spo, LOW); // servo Power on
  for (int dug = 0; dug < f_kug; dug++) {
    // Hauptschleife für die Kugeln
    // 0. schalter immer noch offen?  wenn nicht dug=kug und break
    // 1. Kamera offen
    // 2. Servo hopp
    // 3. warte auf Schuss
    // 4. Berechne V und Aufschlagzeit
    // 5. löse Blitz aus
    // 6. Gib V0 und Versatzzeit aus
    // 7. Servo zurück
    // 8. warte

    //0.
    starttaster();

    if (!sicherheit()) {
      dug = f_kug;
      break;
    }; // Schalter nicht mehr umgelegt LOW bricht ab

    // 1.
    // sende IR-Signal an die Kamera
    kamera_ausloesen();

    // 2.
    // Setze Servo auf Auslösewinkel
    servo_setzen(1, 0);

    // 3.
    // Messung
    while (digitalRead(pin_ls0)) {}; // warte bis LS0 auf LOW geht
    unsigned long st0 = micros();
    // erster Messpunkt gespeichert
    if (debug) {
      Serial.println();
      Serial.print("LS0: "); Serial.println(st0);
    };

    while (digitalRead(pin_ls1)) {}; // warte auf die 2. Lichtschranke LOW FLanke
    unsigned long st1 = micros();
    // zweiter Messpunkt gespeichert
    if (debug) {
      Serial.println();
      Serial.print("LS1: "); Serial.println(st1);
    };
    unsigned long stl = st1 - st0;
    // Zeit über Lichtschranke [us]

    float vku = float(f_len) * float(1000) / float(stl); // [[mm*1000]=um] / [us]
    // Mündungs-V0 in um/us^=m/s

    // 4.
    // Aufschlagzeit mit Latenz und aktuellem Versatz
    unsigned long auf = st1 + ((f_dis * stl) / f_len) ; // Aufschlagzeit in micros()-Einheit
    long ver =  f_ve0 + f_dti * dug; // aktueller Zeitversatz aus Basisversatz und Schritt / Durchgang
    unsigned long aus = (unsigned long) ((long)auf + (long)ver); // wann soll blitz auslösen

    // 5.
    // löse den Blitz aus
    blitz_ausloesen(aus);

    // 6. Ausgaben
    if (debug) {
      Serial.print("stl: "); Serial.println(stl);
      Serial.print("vk0: "); Serial.println(vku);
      Serial.print("f-dis: "); Serial.println(f_dis);
      Serial.print("f-len: "); Serial.println(f_len);
      Serial.print("auf: "); Serial.println(auf);
      Serial.print("aus: "); Serial.println(aus);
      Serial.print("ver: "); Serial.println(ver);
    };

    Wert[4] = dug + 1;
    Wert[5] = (int) (vku * 10.0);
    Wert[6] = (int) ver ;// 1000;
    Wert[7] = (int) (vku * ver / 1000);

    // gib Vo, Runde und Zeitversatz aus; Versatz, V0, Durchgang, Leuchtdauer
    led_setzen (ver, vku, dug, lda);
    // led_setzen (-90, 80.5, 3, 2000);
    Anzeige(lcd_aktiv , prn_aktiv, true);

    // 7.
    // Servo zurück
    servo_setzen(0, sew);
    zieldrehung();

    // 8.
    // warte laz(Latenzzeit) msec (Kameraverschluß, Blitzladazyklus etc)
    delay(laz);
  };

  // halt!
  servo_setzen(0, sew); // Nullposition
  digitalWrite(pin_spo, HIGH); // Servo stromlos
};

//==============================================================================================
// programmteile
void setup () {
  //Drehwahlschalter
  pinMode(pin_clk, INPUT_PULLUP);
  pinMode(pin_dre, INPUT_PULLUP);
  pinMode(pin_but, INPUT_PULLUP);

  // programmteilwahlschalter
  pinMode(pin_prw, INPUT); // der Pin ist mit 100 k auf LOW gelegt und geht HIGH für 'Lauf'

  // Lichtschranke
  pinMode(pin_ls0, INPUT);
  pinMode(pin_ls1, INPUT);

  // BLitz
  pinMode(pin_bli, OUTPUT);
  digitalWrite(pin_bli, HIGH);

  //Seriell
  if (debug) {
    Serial.begin(9600);
    delay(300);
  };
  //LCD
  lcd.begin(20, 4);
  /*
    lcd.clear();
    lcd.setBacklight(HIGH);
    delay(1000);
    lcd.setBacklight(LOW);
    delay(1000);
  */
  lcd.clear();

  Anzeige(lcd_aktiv, prn_aktiv, false);

  //Schrittmotor
  Drehscheibe.setSpeed(drehgeschwindigkeit);

  // IR Diode --> Bibliothek PIN 3 festgelegt; schon initialisiert

  // Servo
  pinMode (pin_spo, OUTPUT);
  digitalWrite(pin_spo, LOW);
  abzug.attach(pin_sst); // Steuerungsausgang Servo zugeordnet
  servo_setzen(se0, sew); // Neutralposition Abzug, 2 Sek. warten

  // weiter-Knopf
  pinMode(pin_wei, INPUT_PULLUP); // geht auf LOW für Schussfreigabe
  pinMode(pin_wli, OUTPUT);

  // 8LED
  lc.shutdown(0, false);
  lc.setIntensity(0, 8);
  lc.clearDisplay(0);

  // slave-Arduino
  pinMode(pin_sla, OUTPUT);
  digitalWrite(pin_sla, HIGH);

}

void loop() {
// led_setzen (-90, 80.5, 3, 2000);
  //Startbildschirm
  startbildschirm();

  // frage solange die Parameter ab, wie der Wahlschalter off ist
  while (!digitalRead(pin_prw)) {
    parameter ();
  };

  zwischenbildschirm();

  // speichere die neuen Parameter im EEPROM
  speichern ();

  // beginne die Bilderstellung
  bildlauf(Wert[0], Wert[1], Wert[2], Wert[3], len);

  // Abschluß
  abschlussbildschirm();
  while (true) {};
}