// 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;

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

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);

  motorsx.run(FORWARD);
  motordx.run(FORWARD);

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

void loop() {
  bool lettura = analogRead(A5) > SOGLIA ? NERO : BIANCO;;

  if ( lettura != old_sx ) {
    passi_sx++;
    old_sx = lettura;
    Serial.print("S:");
    Serial.println(passi_sx);
  }

  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 >= 10 || passi_sx >= 10 ) {
    if (passi_dx > (passi_sx + 1)) {
      if (velocita_dx <= 180)
        velocita_sx++;
      else
        velocita_dx--;

      motordx.setSpeed(velocita_dx);
    }
    else if (passi_sx > ( passi_dx + 1)) {
      if (velocita_sx <= 180)
        velocita_dx++;
      else
        velocita_sx--;
      motordx.setSpeed(velocita_sx);
    }

    passi_sx = passi_dx = 0;
  }

  /*
    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);
      }
    }
  */
}