#include <FastLED.h>
#define NUM_LEDS 2
#define DATA_PIN 9
int pos = 0;
long y = 0;
CRGB leds[NUM_LEDS];


void setup() {
  // put your setup code here, to run once:

  Serial.begin(9600);
  Serial.setTimeout(2000);
  pinMode(6, OUTPUT); //Enable
  pinMode(5, OUTPUT); //Step
  pinMode(4, OUTPUT); //Direction

  pinMode(7, OUTPUT); //Enable
  pinMode(2, OUTPUT); //Step
  pinMode(3, OUTPUT); //Direction


  digitalWrite(6, LOW);
  digitalWrite(7, HIGH); //Low aktiv

   FastLED.addLeds<WS2812B, DATA_PIN, GRB>(leds, NUM_LEDS);
  
}


void loop(){
  leds[0] = CRGB::Red;
  leds[1] = CRGB::Blue;
  FastLED.show();
  String inputData;
  int i;
  y++;
//  delayMicroseconds(15000);
  if (y == 2500) { y = 0; Serial.print("timeout\r\n");}
  if(Serial.available() >= 2) {
     y=0;
     inputData = Serial.readString();
     leds[0] = CRGB::Red;
     leds[1] = CRGB::Red;
     FastLED.show();
     switch (inputData.charAt(0)) {
      case 'a':
        StepperY(0);
        StepperX(int(inputData.charAt(1)));  //Menge
        break;
      case 'b':
        StepperY(1);
        StepperX(int(inputData.charAt(1)));  //Menge
        break;
      case 'c':
        StepperY(2);
        StepperX(int(inputData.charAt(1)));  //Menge
      break;
      case 'd':
        StepperY(3);
        StepperX(int(inputData.charAt(1)));  //Menge
        break;
      case 'e':
        StepperY(4);
        StepperX(int(inputData.charAt(1)));  //Menge
      break;
      case 'f':
        StepperY(5);
        StepperX(int(inputData.charAt(1)));  //Menge
        break;
      case 'g':
        StepperY(6);
        StepperX(int(inputData.charAt(1)));  //Menge
        break;
      case 'h':
        StepperY(7);
        StepperX(int(inputData.charAt(1)));  //Menge
        break;
      case 'i':
        StepperY(8);
        StepperX(int(inputData.charAt(1)));  //Menge
        break;
      case 'j':
        StepperY(9);
        StepperX(int(inputData.charAt(1)));  //Menge
        break;

      case 'z':
        leds[0] = CRGB::Green;
        leds[1] = CRGB::Green;
        FastLED.show();
        for (i = 0; i < 550; i++) delayMicroseconds(15000);
        Serial.print("Fertig\r\n");
        break;
      default: 
        for (i = 0; i < 550; i++) delayMicroseconds(15000);
        Serial.print("error\r\n");
        Serial.print(inputData);
        leds[0] = CRGB::Yellow;
        leds[1] = CRGB::Yellow;
        inputData = "";
        break; 
     }
  }
}


void StepperX(int Menge) {      //Dosiermenge
  long int Index;
  int i;
  // put your main code here, to run repeatedly:
    digitalWrite(7, LOW);
    for (Index = 0; Index < 2000; Index++)
    {
      digitalWrite(2, HIGH);
      delayMicroseconds(1500);
      digitalWrite(2, LOW);
      delayMicroseconds(1500);
    }
    digitalWrite(3, HIGH);
    Menge = Menge -48;          //Char to int
    Serial.print("Menge");
    Serial.print(Menge);
    Serial.print("\r\n");
      if (Menge > 0 && Menge < 10) {
        for (i = 0; i < Menge; i++){
          for (Index = 0; Index < 55; Index++) delayMicroseconds(15000);
        }
      }
    for (Index = 0; Index < 2000; Index++)
    {
      digitalWrite(2, HIGH);
      delayMicroseconds(1500);
      digitalWrite(2, LOW);
      delayMicroseconds(1500);
    }
    digitalWrite(7, HIGH);
    digitalWrite(3, LOW);
    for (Index = 0; Index < 55; Index++) delayMicroseconds(15000);
    Serial.print("Fertig\r\n");

}

void StepperY(int soll) {  //Position
  delayMicroseconds(1500);
  long int Index;
    digitalWrite(6, LOW);
    while (soll > pos) {
          Serial.print("rechts");

//      for (Index = 0; Index < 55; Index++) delayMicroseconds(15000);
      for (Index = 0; Index < 500; Index++)
      {
        digitalWrite(5, HIGH);
        delayMicroseconds(1500);
        digitalWrite(5, LOW);
        delayMicroseconds(1500);
      }
    pos++;
    }
    while (soll < pos) {
    
        digitalWrite(4, HIGH); //Links Drehen
    
        Serial.print("links");

//        for (Index = 0; Index < 55; Index++) delayMicroseconds(15000);
        for (Index = 0; Index < 500; Index++)
        {
        digitalWrite(5, HIGH);
        delayMicroseconds(1500);
        digitalWrite(5, LOW);
        delayMicroseconds(1500);
        }
      digitalWrite(4, LOW); //Wieder normale richtung
      pos--;
    }
    
    digitalWrite(6, HIGH);
  //  for (Index = 0; Index < 55; Index++) delayMicroseconds(15000);
    Serial.print("pos: ");
    Serial.print(pos, DEC);
  
}