#include <AccelStepper.h>  // library für die Stepmotoren
#include <NewPing.h>      // library für die Ultraschallsensoren

#define SONAR_NUM     4 // Anzahl der Ultraschallsensoren
#define MAX_DISTANCE 100 // Maximale Entfernung, die von den Sensoren erfasst wird (in cm). Wenn Entfernung größer als 100 cm wird 0 ausgegeben
#define PING_INTERVAL 33 // Zeit zwischen den Messungen der einzelnen Sensoren (min 29 ms)

AccelStepper stepper1 (1,51,53); //Linker Stepmotor
AccelStepper stepper2 (1,50,52); //Rechter Stepmotor
AccelStepper stepperX (1,5, 7); // Motor der X-Achse des Malgestells
AccelStepper stepperY (1,3, 1); // Motor der Y-Achse des Malgestells

unsigned long pingTimer[SONAR_NUM]; // Array für die sequentielle Auslesung der Ultraschallsensoren
unsigned int cm[SONAR_NUM];         // Array um die Distanz des jeweiligen Sensors zu speichern
uint8_t currentSensor = 0;          

NewPing sonar[SONAR_NUM] = {     // Array für die Pins der Ultraschallsensoren
  NewPing(37, 36, MAX_DISTANCE), // z.B. Trigger auf Pin 37, Echo auf Pin 36, maximale Entfernung 100 cm
  NewPing(33, 32, MAX_DISTANCE), // Momentan läuft das Programm nur mit den beiden vorderen Sensoren also cm[0] und cm[1]
  NewPing(41, 40, MAX_DISTANCE),
  NewPing(45, 44, MAX_DISTANCE)
};

void setup() {
  Serial.begin(115200);
  pingTimer[0] = millis() + 75;           // kurzer delay bevor die ersten Messungen gemacht werden
  for (uint8_t i = 1; i < SONAR_NUM; i++) // Startintervalle für die Sensoren
   { pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;}
   
  stepper1.setMaxSpeed(5000);   // init für Stepmotor 1
  stepper1.setAcceleration(1000);
  stepper1.setSpeed(5000 );
  
  stepper2.setMaxSpeed(10000);  // init für Stepmotor 2
  stepper2.setAcceleration(1000);
  stepper2.setSpeed(10000); 
  
  stepperX.setMaxSpeed(20000); // init für Stepmotor X
  stepperX.setAcceleration(1000);
  stepperX.setSpeed(20000);
  
  stepperY.setMaxSpeed(20000); // init für Stepmotor Y
  stepperY.setAcceleration(1000);
  stepperY.setSpeed(20000);
}

boolean Bewegung=false;
boolean stepper1_on=false;
boolean stepper2_on=false;

char cSignal;
char koordinaten;

void loop() {
  
  stepper1.run();  // Der Startbefehl für die Motoren muss mindestens einmal in der loop aufgerufen werden damit die Motoren sich bewegen
  stepper2.run();
  
  if(cm[0]<=3 && cm[1] <=3 && cm[0]!=0 && cm[1]!=0){ // Stoppt den Roboter wenn vor unmittelbar vor beiden vorderen Sensoren ein objekt ist (hoffentlich eine wand)
  
    stepper1.stop();
    stepper2.stop();
    Bewegung=false;
    
  }
  else{
    Bewegung=true;
  }
  if(Bewegung){               
    movement ();
  }
  else{
    sketch ();
  }
  for (uint8_t i = 0; i < SONAR_NUM; i++) { // liest alle Sensoren nacheinander ab
    if (millis() >= pingTimer[i]) {         // ist es Zeit den Sensor abzulesen
      pingTimer[i] += PING_INTERVAL * SONAR_NUM;  // speichert die Zeit wann der Sensor das nächste mal abgelesen werden soll
      if (i == 0 && currentSensor == SONAR_NUM - 1) oneSensorCycle(); // Der Sensor kann abgelesen werden
      sonar[currentSensor].timer_stop();          // stoppt den Timer vor der Messung
      currentSensor = i;                          
      cm[currentSensor] = 0;                      // Setzt die Entfernung auf 0 für den Fall dass kein echo empfangen wird
      sonar[currentSensor].ping_timer(echoCheck); // Misst die Entfernung am Sensor i
    }
  }
 
}

void echoCheck() { // Wenn ein Echo eingefangen wurde speichert er den Wert unter cm[] und rechnet die gemessene Zeit in cm um ("/US_ROUNDTRIP_CM")
  if (sonar[currentSensor].check_timer())
    cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
}

void oneSensorCycle() { // Werte wurden abgelesen und gespeichert hier werden sie auf dem Serial Monitor ausgegeben
  for (uint8_t i = 0; i < SONAR_NUM; i++) {
    Serial.print(i);
    Serial.print("=");
    Serial.print(cm[i]);
    Serial.print("cm ");
  }
  Serial.println();                        // Position des Steppers wird auf dem Serial Monitor ausgegeben
  Serial.print(stepper1.currentPosition());
  Serial.print("\t");
  Serial.println(stepper2.currentPosition());
}
void movement(){
    if(cm[1] <=3 && cm[1]!=0){  // stepper 2 fährt nicht weiter wenn ein objekt 3 cm oder weniger vor dem Sensor ist
    
    stepper2_on=false;
  }
  else{
    stepper2_on=true;
  }
  if(cm[0]<=3 && cm[0]!=0){   // stepper 1 fährt nicht weiter wenn ein objekt 3 cm oder weniger vor dem Sensor ist
    stepper1_on=false;
  }
  else{
    stepper1_on=true;
  }
  if(stepper2_on){      //stepper 2
  if(cm[1] <=100 && cm[1]!=0){
   if(stepper2.distanceToGo()==0){
    stepper2.move((cm[1]-2)*29);   //(abstand - 2 cm) um von cm in schritte umzurechnen *29
    stepper2_on=false;
   } 
  }
  else{     // wenn die Distanz gleich 0, also größer 100 cm dreht sich der Roboter solange bis er etwas findet
    if(stepper2.distanceToGo()==0){
    stepper2.move(200);
   }
   stepper2_on=false;
  }
  }
  if(stepper1_on){    //stepper 1
  if(cm[0] <=100 && cm[0]!=0){
  if(stepper1.distanceToGo()==0){
    stepper1.move((cm[0]-2)*29);   //(abstand - 2 cm) um von cm in schritte umzurechnen *29
    stepper1_on=false;
  }
  }
  else{                            // wenn die Distanz gleich 0, also größer 100 cm dreht sich der Roboter solange bis er etwas findet
    if(stepper1.distanceToGo()==0){
    stepper1.move(800);
  }
   stepper1_on=false;
  
  }
  }
} 
void sketch(){
  // hier wird der Malpart implementiert sobald dieser fertig ist
}