// Adafruit Motor shield library
// copyright Adafruit Industries LLC, 2009
// this code is public domain, enjoy!

#include <AFMotor.h>

AF_DCMotor motorsx(1);
AF_DCMotor motordx(2);
#define NERO 1
#define BIANCO 0
#define SOGLIA 400

int passi_sx = 0;
int passi_dx = 0;
int passi;

bool old_sx, old_dx;
int velocita_sx = 255;
int velocita_dx = 255;
char motore;

#define FERMO 0
#define CONTANDO 1

byte stato = FERMO;

void setup() {
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Motor test!");

  // turn on motor
  motordx.setSpeed(255);
  motorsx.setSpeed(255);

  old_sx = analogRead(A4) > SOGLIA ? NERO : BIANCO;
  old_dx = analogRead(A5) > SOGLIA ? NERO : BIANCO;
}

void loop() {
  if (stato == FERMO) {
    if (Serial.available() > 0) {
      motore = Serial.read();
      passi = Serial.parseInt();
      stato = CONTANDO;
      motorsx.run(FORWARD);
      motordx.run(FORWARD);
    }
  }
  else if (stato == CONTANDO) {


    if (motore == 's') {
      bool lettura = analogRead(A5) > SOGLIA ? NERO : BIANCO;
      if ( lettura != old_sx ) {
        passi_sx++;
        old_sx = lettura;
        Serial.print("S:");
        Serial.println(passi_sx);
      }
    }
    if (motore == 'd') {
      bool lettura = analogRead(A4) > SOGLIA ? NERO : BIANCO;;

      if ( lettura != old_dx ) {
        passi_dx++;
        old_dx = lettura;
        Serial.print("D:");
        Serial.println(passi_dx);
      }
    }
    if ( passi_dx >= passi || passi_sx >= passi ) {
      passi_dx = passi_sx = 0;
      motorsx.run(RELEASE);
      motordx.run(RELEASE);
      stato = FERMO;
    }

    /*
      if (Serial.available() > 0) {
        char c = Serial.read();
        if (c == 'r')
        {
          passi_sx = passi_dx = 0;
        }
        if (c == 's') {
          if (velocita > 0) velocita--;
          motordx.setSpeed(velocita);
          motorsx.setSpeed(velocita);
        }
        if (c == 'f') {
          if (velocita < 255) velocita++;
          motordx.setSpeed(velocita);
          motorsx.setSpeed(velocita);
        }
      }
    */
  }
}