/* --------------------------------------------------
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

const int analogInPin = 34; // Eingang Poti, Schrittgroesse
const int PUL = 14;  // D5  Motor Step
const int DIR = 4;  // D2   Motor Drehrichtung
const int ENA = 15;  // D8  Motor aktiv
const int PinXrf = 5; //D?? Steuerung XRF
const int buttonPinA  = 12; //D6  Button vor Bewegung
const int buttonPinB  = 16; //D0  Button zurück Bewegung
const int buttonPinKalib  = 27; //D??? Button Kalibrations
const int buttonPinEnd  = 26; //D??? Endschalter
const int buttonPinAuto  = 18; //D??? Button Automatik
const int buttonPinStop = 19; // D?? Button Stop
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
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 = 5; //signalänge für das xrf in sek.
int MessP1 = 2; // Pausenlänge vor der Messung in sek.
int MessP2 = 2; // Pausenlänge nach der Messung in sek.
int MessN = 0;
int Atext = 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);
  if (buttonStateStop == LOW){
    stop();
  }
  if (buttonStateA == LOW){
    X_AxisA();
  }
  if (buttonStateB == LOW){
    X_AxisB();
  }    
  if (buttonStateKalib == LOW){
    position =0;
    Atext = 0;
    kalib = 1;
    Anzeige();
  } 
  if (buttonStateAuto == LOW){
    automatik();
  }                  
}

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);
  display.print("Pos: ");
  display.print(position);
  display.print(" cm");
  
  display.setCursor(0, 60);
  switch (Atext){
    case 0:
    if (kalib == 0){ 
      display.print("Nicht Kalibriert");
    } else if (kalib == 1){ 
      display.print("Kalibriert");
    } else if (kalib == 2){ 
      display.print("erst Kalibrieren");
    }
    break;
    case 1:
      display.print("Messung...");
      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 (kalib == 1){
    halten = 0;
    Atext=1;
    Anzeige();
    MessN = (posMax - position)/resolution;
    for (int i =0; i< MessN; i++){
      if (halten == 0){
        delayMicroseconds(MessP1*1000000);    
        digitalWrite(PinXrf, HIGH);
        delayMicroseconds(MessL*1000000);
        digitalWrite(PinXrf,LOW);
        delayMicroseconds(MessP2*1000000);
        X_AxisA();
      }
    }
    if (halten == 0){
      delayMicroseconds(MessP1*1000000);    
      digitalWrite(PinXrf, HIGH);
      delayMicroseconds(MessL*1000000);
      digitalWrite(PinXrf,LOW);
      delayMicroseconds(MessP2*1000000);
    }
    Atext = 2;
  }
 else if (kalib == 0){
  kalib = 2;
  Anzeige();
 }
}
void stop(){
Atext = 3;
Anzeige();
halten = 1;
 digitalWrite(ENA, HIGH);
 digitalWrite(PinXrf,LOW);
}