/* cerca_scuro.nqc 
 *
 * Robot autonomo.
 * 
 * Esplora il campo (che si suppone essere un tavolo piano) 
 * finche' non trova una regione scura.
 *
 * Se incontra un bordo, arretra di un po', cambia direzione 
 * e poi riprende da capo.
 *
 * Utilizza il differential drive.
 * A: Motore Destro
 * B: vuoto
 * C: Motore Sinistro
 * 1: Sensore Touch
 * 2: Sensore Light
 * 3: vuoto
 */

#define TRUE 1
#define BRAKE_TIME 5
#define TURN_TIME 15
#define CRUISE_SPEED 4
#define CHIARO 45

#define SENS_LIGHT SENSOR_2
#define SENS_TOUCH1 SENSOR_1
//#define SENS_TOUCH2 SENSOR_3
#define DISP_SENS_LIGHT DISPLAY_SENSOR_2


#define DX_SPEED(X) SetPower(OUT_A,X);
#define DX_DIR(X) SetDirection(OUT_A,X);
#define DX_MODE(X) SetOutput(OUT_A,X);
#define SX_SPEED(X) SetPower(OUT_B,X);
#define SX_DIR(X) SetDirection(OUT_B,X);
#define SX_MODE(X) SetOutput(OUT_B,X);
#define TUTTI_SPEED(X) SetPower(OUT_A+OUT_B,X);
#define TUTTI_DIR(X) SetDirection(OUT_A+OUT_B,X);
#define TUTTI_MODE(X) SetOutput(OUT_A+OUT_B,X);

int FINE = 0;

void vai_diritto() {
  TUTTI_SPEED(CRUISE_SPEED); 
  TUTTI_DIR(OUT_FWD);
  TUTTI_MODE(OUT_ON);
}

void ferma (){
  TUTTI_MODE(OUT_OFF);
  Wait(BRAKE_TIME);
  TUTTI_MODE(OUT_FLOAT);   
}

void vai_indietro() {
  TUTTI_SPEED(CRUISE_SPEED); 
  TUTTI_DIR(OUT_REV);
  TUTTI_MODE(OUT_ON);
}

void gira_a_destra (){
  TUTTI_SPEED(CRUISE_SPEED);   
  DX_DIR(OUT_REV);  
  SX_DIR(OUT_FWD);
  TUTTI_MODE(OUT_ON);
  Wait(TURN_TIME);
}

task evita_bordo() {
  while(TRUE){
    while(SENS_TOUCH1 == 0); // ... finche' non trovi un bordo ...
    ferma(); // ... e fermati!
    //cputs("bordo");
    SelectDisplay(DISPLAY_WATCH);
    SetWatch(60,06); // non posso visualizzare stringhe di caratteri!
    vai_indietro(); // Torna indietro ... 
    while(SENS_TOUCH1 == 1); // ... finche' non sei lontano dal bordo ...
    vai_indietro(); // ... e poi ancora per un po'.
    Wait(100);
    gira_a_destra(); // Cambia direzione.
    vai_diritto(); // Avanti ...
  }
}

task trova_scuro() {
  Wait(1);
  while(SENS_LIGHT >= CHIARO); // ... finche' non trovi scuro
  ferma(); // ... e fermati!
  FINE = 1;
}

task main()
{
  int L;

  // Configurazione
  SetSensor(SENS_LIGHT, SENSOR_LIGHT);
  SetSensor(SENS_TOUCH1, SENSOR_TOUCH);

  vai_diritto(); // Avanti ...
  start evita_bordo; // attiva il task di rilevamento del bordo
  start trova_scuro;  // attiva il task di rilevamento del colore

//  cputs("run");
  SelectDisplay(DISPLAY_WATCH);
  SetWatch(12,34); // non posso visualizzare stringhe di caratteri!
  PlayTone(50,40);

  FINE = 0;
  while(FINE == 0){
    Wait(10);
    SelectDisplay(DISP_SENS_LIGHT);
    Wait(10);
//    cputs("run");
    SelectDisplay(DISPLAY_WATCH);
    SetWatch(12,34); // non posso visualizzare stringhe di caratteri!
  }
  Wait(10);
  StopAllTasks();
  
//  cputs("stop");
  SelectDisplay(DISPLAY_WATCH);
  SetWatch(11,11); // non posso visualizzare stringhe di caratteri!
}