#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

//EnablePIN for on/off all Steppers
#define EnPin 23

//Stepper 1
#define directionPin1 16
#define stepPin1 17
//Stepper 2
#define directionPin2 18
#define stepPin2 19

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for SSD1306 display connected using I2C
#define OLED_RESET     -1 // Reset pin
#define SCREEN_ADDRESS 0x3C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

//Rückwärt mit Kontroller fehlt noch mit direckton LOW...Kann (kleiner Grööser???) und Betrag beim speed
//denke muss mit stick gereglt werden 1Y und 2Y
// minimal bewgung im COde ignoreirein 10=>Y =0 else Y 

int pulseWidthMicros = 20;  // microsecondo
int millisbetweenSteps1 = 25; // milliseconds - or try 10-250 (or so) 100 for slower steps
int millisbetweenSteps2 = 20; // milliseconds - or try 10-250 (or so) 100 for slower steps

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  Serial.println("Starting StepperTest");
  pinMode(directionPin1, OUTPUT);
  pinMode(stepPin1, OUTPUT);
  pinMode(directionPin2, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(EnPin, OUTPUT);
  digitalWrite(EnPin, LOW);

  // initialize the OLED object
  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;); // Don't proceed, loop forever
  }
  // Clear the buffer.
  display.clearDisplay();

  // Display Text //müsste in den Loop /aktualise//ohne den stepper zu verzögern
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.println("speed1");
  display.println(millisbetweenSteps1);
  display.setTextSize(2);
  display.println("speed2");
  display.println(millisbetweenSteps2);
  display.display();

  //Core more
  xTaskCreatePinnedToCore (
    loop2,     // Function to implement the task
    "loop2",   // Name of the task
    1000,      // Stack size in bytes
    NULL,      // Task input parameter
    0,         // Priority of the task
    NULL,      // Task handle.
    0          // Core where the task should run
  );
}
//pararlelverzögerung der steps fehlt noch
void loop() {
  digitalWrite(directionPin1, HIGH);
  digitalWrite(stepPin1, HIGH);
  delayMicroseconds(pulseWidthMicros);
  digitalWrite(stepPin1, LOW);
  delay(millisbetweenSteps1);
}
// the loop2 function also runs forver but as a parallel task
void loop2 (void* pvParameters) {
  while (1) {
   digitalWrite(directionPin2, HIGH);
   digitalWrite(stepPin2, HIGH);
   delayMicroseconds(pulseWidthMicros);
   digitalWrite(stepPin2, LOW);
   delay(millisbetweenSteps2);
  }
}