/* --------------------------------------------------
toDo
- endschalter position
- kalibmodus
-------------------------------------------------- */
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Fonts/FreeSerif9pt7b.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
// GPIO Belegung
const int analogInPin = 34; // Eingang Poti, Schrittgroesse
const int PUL = 14;  //  Motor Step
const int DIR = 26;  //    Motor Drehrichtung
const int ENA = 25;  //  Motor aktiv
const int PinXrf = 17; // Steuerung XRF
const int buttonPinA  = 15; //  Button vor Bewegung
const int buttonPinB  = 16; //  Button zurück Bewegung
const int buttonPinKalib  = 27; // Button Kalibrations
const int buttonPinEnd  = 32; // Endschalter
const int buttonPinAuto  = 18; // Button Automatik
const int buttonPinStop = 19; //  Button Stop
// Feste Variablen
const int rota = 3200; // steps fuer eine umdrehung
const int vmot = 100; // Motorgeschwindigkeit (ms 2 Steps)
const int posMax = 105; //Maximale Position der X-Achse
const float posMin = -0.5; //Minimale Position auf der X-Achse
// freie Variablen
int sensorValue = 0;
int buttonStateA = 0;
int buttonStateB = 0;
int buttonStateKalib = 0;
int buttonStateEnd = 0;
int buttonStateAuto = 0;
int buttonStateStop = 0;
float centi = (rota)/4; //steps fuer einen Zentimeter
float resolution = 1;   //abstaende zwischen den Messpunkten in cm
int steps = centi*resolution;
float position = 0;
int kalib = 0;
int halten = 0;
int MessL = 5000; //signalänge für das xrf in msek.
int MessP1 = 2000; // Pausenlänge vor der Messung in msek.
int MessP2 = 2000; // Pausenlänge nach der Messung in msek.
int MessN = 0;
int Atext = 0; 
int messung = 0;
unsigned long STime;
unsigned long NTime;
unsigned long P1Time;
unsigned long P2Time;
unsigned long LTime;
int mmm = 0;

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

void setup() {
  pinMode(PUL, OUTPUT);
  pinMode(DIR, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(PinXrf, OUTPUT);
  pinMode (buttonPinA, INPUT_PULLUP);
  pinMode (buttonPinB, INPUT_PULLUP);
  pinMode (buttonPinKalib, INPUT_PULLUP);
  pinMode (buttonPinEnd, INPUT_PULLUP); 
  pinMode (buttonPinAuto, INPUT_PULLUP);
  pinMode (buttonPinStop, INPUT_PULLUP);
  digitalWrite(PUL, LOW);
  digitalWrite(DIR, LOW);
  digitalWrite(ENA, HIGH);
  digitalWrite(PinXrf, LOW);
  Serial.begin(115200);
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  delay(500);
  sensorValue = analogRead(analogInPin);
}

void loop() {
  sensor();
  buttonStateA = digitalRead (buttonPinA);
  buttonStateB = digitalRead (buttonPinB);
  buttonStateKalib = digitalRead (buttonPinKalib);
  buttonStateAuto = digitalRead (buttonPinAuto);
  buttonStateStop = digitalRead (buttonPinStop);
  buttonStateEnd = digitalRead (buttonPinEnd);
  if (buttonStateStop == LOW){
    stop();
  }
  if (buttonStateA == LOW){
    X_AxisA();
  }
  if (buttonStateB == LOW){
    X_AxisB();
  } 
  if (buttonStateEnd == LOW){
    endschalter();
  }   
  if (buttonStateKalib == LOW){
    position =0;
    Atext = 0;
    kalib = 1;
    Anzeige();
  } 
  if (buttonStateAuto == LOW){
   messung =1;
   halten = 0;
   STime = millis();
   Serial.println(STime);
   MessN = 1+((posMax - position)/resolution);
   Serial.println(MessN);
  }
  automatik();
  Anzeige();
}
void sensor(){
  sensorValue = analogRead(analogInPin);
  if (sensorValue <=500){ 
    resolution = 0.5;
  } else if (sensorValue<=1000){
    resolution = 1;
  } else if (sensorValue<=1500){
    resolution = 2;
  } else if (sensorValue<=2000){
    resolution = 3;
  } else if (sensorValue<=2500){
    resolution = 4;
  } else if (sensorValue<=3000){
    resolution = 5;
  } else if (sensorValue<=3500){
    resolution = 10;
  } else if (sensorValue<=4200){
    resolution = 15;
  }
  Anzeige();
}
void X_AxisA(){
 halten = 0;
 digitalWrite(DIR, HIGH);
 digitalWrite(ENA, LOW);
 steps = centi*resolution;
  for (int i=0; i < steps; i++) {  
    //Abschaltung
    buttonStateEnd = digitalRead (buttonPinEnd);
    buttonStateStop = digitalRead (buttonPinStop);
    if (buttonStateEnd == LOW){
      endschalter();
      break;
    }
    if (buttonStateStop == LOW){
      stop();
      break;
    }
    if (kalib == 1 && position > posMax){
      break;
    }
    //Schritte
    digitalWrite(PUL, HIGH);
    delayMicroseconds(vmot);
    digitalWrite(PUL,LOW);
    delayMicroseconds(vmot);
    position = position + (1/centi);
  } 
  Anzeige(); 
}
void X_AxisB(){
  halten = 0;
  digitalWrite(DIR, LOW);
  digitalWrite(ENA, LOW);
  steps = centi*resolution;
  for (int i=0; i < steps; i++) {  
    //Abschaltung
    buttonStateEnd = digitalRead (buttonPinEnd);
    buttonStateStop = digitalRead (buttonPinStop);
    if (buttonStateEnd == LOW){
      endschalter();
      break;
    }
    if (buttonStateStop == LOW){
      stop();
      break;
    }
    if (kalib == 1 && position < posMin){
      break;
    }
    //Schritte
    digitalWrite(PUL, HIGH);
    delayMicroseconds(vmot);
    digitalWrite(PUL,LOW);
    delayMicroseconds(vmot);  
    position = position - (1/centi);
  } 
  Anzeige();
}
void endschalter(){
  Atext = 4;
  digitalWrite(ENA, HIGH);
  halten= 1;
  Anzeige();
}
void Anzeige(){
  display.clearDisplay();
  //display.setTextSize(1);
  display.setFont(&FreeSerif9pt7b);
  display.setTextColor(WHITE);
  
  display.setCursor(0, 15);
  display.print("Schritt: ");
  display.print(resolution);
  display.print(" cm");
  
  display.setCursor(0, 35);
  if (kalib == 0){ 
      display.print("Nicht Kalibriert");
    } else if (kalib == 1){ 
      display.print("Pos: ");
      display.print(position);
      display.print(" cm");
    } else if (kalib == 2){ 
      display.print("erst Kalibrieren");
    }
 
  
  display.setCursor(0, 60);
  switch (Atext){
    case 0:
      if (kalib == 0){ 
        display.print("Auf 0.0 schieben");
      } else if (kalib == 1){ 
        display.print("Kalibriert");
      } else if (kalib == 2){ 
      display.print("Auf 0.0 schiebe");
    }
      break;
    case 1:
      display.print(MessN);
      display.print(" Messungen");
      break; 
    case 2:
      display.print("Mes. abgeschlossen");
      break; 
    case 3:
      display.print("Stop!");
      break;
    case 4:
      display.print("Endschalter!");
      break;
  }
  display.display(); 
}
void automatik(){
  if (messung == 1 && kalib == 0){
  kalib = 2;
  Anzeige();
  messung = 0;
 }
 if (kalib == 1 && messung == 1 && MessN >= 0 && halten == 0){
  NTime = millis();
  P1Time = MessP1+STime;
  P2Time = MessP1+MessL+MessP2+STime;
  LTime = MessP1+MessL+STime;
  //Serial.print("NTime ");
  //Serial.println(NTime);
    if (NTime >= P1Time && NTime <= LTime){
      if(digitalRead(PinXrf) == LOW){
      Serial.println(" XRF Messung");
      digitalWrite(PinXrf, HIGH);
      Atext = 1;
      mmm=1;
      }  
    } else if (NTime > LTime && NTime <= P2Time){
      if(digitalRead(PinXrf) == HIGH){
          Serial.print(" Messpause");
          digitalWrite(PinXrf, LOW);
        }
    } else if (NTime > P2Time && mmm == 1){
        mmm=0;
          MessN = MessN-1;
          STime = millis(); 
        if(MessN >= 1){
          Serial.print(" Naechster Messpunkt ");
          Serial.print(MessN);
          X_AxisA();
          
        }
       }
  }
  if (kalib == 1 && messung == 1 && MessN == 0 && halten == 0){
    Atext = 2;
    messung = 0;
  }
}
void stop(){
  Atext = 3;
  Anzeige();
  halten = 1;
  messung =0;
  digitalWrite(ENA, HIGH);
  digitalWrite(PinXrf,LOW);
}