#include <AccelStepper.h>

int leBigMove[2] = {20000, 20000};
int stepperDestination[2];
int commande[2] = {0, 0};
int bottone = 13;
int bottone2 = 12;
int deathButton = 11;
int motor1DirPin = 2;
int motor1StepPin = 3;
int motor2DirPin = 4;
int motor2StepPin = 5;
int movSensore = 10;
int startGame = 0;
int numMovs = 0;
int stopGame = 0;
AccelStepper stepper1(1, motor1StepPin, motor1DirPin);
AccelStepper stepper2(1, motor2StepPin, motor2DirPin);

uint32_t ts1[2] = {millis(), millis()};
uint32_t ts2[2] = {millis()+1000001, millis()+1000001};

void setup() {
    delay(1000);
  pinMode(bottone, INPUT);
  pinMode(bottone2, INPUT);
  pinMode(deathButton, INPUT);
  Serial.begin(9600);
  stepper1.setSpeed(50);
  stepper1.setMaxSpeed(50);
  stepper1.setAcceleration(100);
  stepper2.setSpeed(50);
  stepper2.setMaxSpeed(50);
  stepper2.setAcceleration(100);
}

void loop() {
  if(!stopGame) {
    int checkMov = digitalRead(movSensore);

    if(checkMov == HIGH && numMovs < 3) {
      numMovs++;
      delay(500);
    }

    if(startGame == 0 && checkMov == HIGH && numMovs > 2) {
      Serial.println("Movimento rilevato");
      stepper1.setSpeed(50);
      stepper1.moveTo(stepperDestination[0]);
      stepper1.run();

      stepper2.setSpeed(50);
      stepper2.moveTo(stepperDestination[1]);
      stepper2.run();
      stepperDestination[0] = leBigMove[0];
      stepperDestination[1] = leBigMove[1];
      startGame = 1;
    }

    if(startGame) {
      int switchs[2] = {digitalRead(bottone), digitalRead(bottone2)};
      if (switchs[0] == LOW) {
        commande[0] = 0;
      }  else { // In caso il bottone venga pressato
        if(ts2[0]-ts1[0] > 500) {
          if (commande[0] == 0) {
            stepper1.setSpeed(0);
            if (stepperDestination[0] < 0) {
              stepper1.moveTo(stepperDestination[0]);
              stepper1.run();
              stepperDestination[0] = leBigMove[0];
              Serial.println("normal");
            } else {
              stepper1.moveTo(-stepperDestination[0]);
              stepper1.run();
              stepperDestination[0] = -leBigMove[0];
              Serial.println("inverse");
            }
            commande[0] = 1;
          }
          ts1[0] = ts2[0];
        }

      }
      ts2[0] = millis();

      if (switchs[1] == LOW) {
        commande[1] = 0;
      } else { // In caso il bottone venga pressato
        if(ts2[1]-ts1[1] > 500) {
          if (commande[1] == 0) {
            stepper2.setSpeed(0);
            if (stepperDestination[1] < 0) {
              stepper2.moveTo(stepperDestination[1]);
              stepper2.run();
              stepperDestination[1] = leBigMove[1];
              Serial.println("normal2");
            } else {
              stepper2.moveTo(-stepperDestination[1]);
              stepper2.run();
              stepperDestination[1] = -leBigMove[1];
              Serial.println("inverse2");
            }
            commande[1] = 1;
          }
          ts1[1] = ts2[1];
        }

      }
      ts2[1] = millis();

      int is_death = digitalRead(deathButton);
      if(is_death == HIGH) {
        stepper1.setSpeed(0);
        stepper2.setSpeed(0);
        startGame = 0;
        numMovs = 0;
        Serial.println("morto");
        stopGame = 1;
      }

      stepper1.moveTo(stepperDestination[0]);
      stepper1.run();
      stepper2.moveTo(stepperDestination[1]);
      stepper2.run();
    }
  }
}