/* cerca_chiaro.c 
 *
 * 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
 */


/* per l'utilizzo dell'emulatore */
#ifdef EMULEGOS
#  include "emuLegOs.h"
#else
#  include <conio.h>
#  include <unistd.h>
#  include <dsensor.h>
#  include <dmotor.h>
#  include <dsound.h>
#endif

#ifdef EMULEGOS
#  define RCXMAIN rcx_main
#else
#  define RCXMAIN main
#endif


#define TRUE 1
#define BRAKE_TIME 500
#define TURN_TIME 1500
#define CRUISE_SPEED 100
#define SCURO 43

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


#define DX_SPEED(X) motor_a_speed(X)
#define DX_DIR(X) motor_a_dir(X)
#define SX_SPEED(X) motor_c_speed(X)
#define SX_DIR(X) motor_c_dir(X)

wakeup_t sensor_press_wakeup(wakeup_t data);
wakeup_t sensor_release_wakeup(wakeup_t data);
wakeup_t light_wakeup(wakeup_t data);

void vai_diritto();
void ferma(int tempo);
void vai_indietro();
void gira_a_destra(int tempo);

int evita_bordo(int argc, char **argv);
int trova_scuro(int argc, char **argv);

int FINE;

// SUONI
static const note_t nota_bassa_corta = {0,  2};

int RCXMAIN(int argc, char **argv) {

  pid_t pid_touch, pid_light;
  int L;

  vai_diritto(); // Avanti ...
  pid_touch = execi(&evita_bordo, 0, NULL, 2, DEFAULT_STACK_SIZE);  // attiva il task di rilevamento del bordo

  pid_light = execi(&trova_scuro, 0, NULL, 10, DEFAULT_STACK_SIZE);  // attiva il task di rilevamento del colore

  FINE = 0;

  cputs("run");
  dsound_play(&nota_bassa_corta);

  while(FINE==0){
    sleep(1);
    L = LIGHT(SENS_LIGHT);
    lcd_int(L);
    sleep(1);
    cputs("run");
  }
  sleep(1);

  kill(pid_touch);
  kill(pid_light);
  
  cputs("stop");
  exit(0);
}


wakeup_t sensor_press_wakeup(wakeup_t data) {
	return TOUCH(SENS_TOUCH1);
}

wakeup_t sensor_release_wakeup(wakeup_t data) {
	return TOUCH(SENS_TOUCH1);
}

wakeup_t light_wakeup(wakeup_t data) {
	return LIGHT(SENS_LIGHT)>SCURO;
}


void vai_diritto() {
  DX_SPEED(CRUISE_SPEED); 
  SX_SPEED(CRUISE_SPEED);
  DX_DIR(fwd);
  SX_DIR(fwd);
};

void ferma(int tempo) {
  DX_DIR(brake);   
  SX_DIR(brake);
  msleep(tempo);
  DX_DIR(off);   
  SX_DIR(off);
};

void vai_indietro() {
  DX_SPEED(CRUISE_SPEED); 
  SX_SPEED(CRUISE_SPEED);
  DX_DIR(rev);
  SX_DIR(rev);
};

void gira_a_destra(int tempo) {
      DX_SPEED(CRUISE_SPEED);   
      SX_SPEED(CRUISE_SPEED);
      DX_DIR(rev);  
      SX_DIR(fwd);
      msleep(tempo);
};

int evita_bordo(int argc, char **argv){
  while(TRUE){
    wait_event(&sensor_press_wakeup,0); // ... finche' non trovi un bordo ...
    ferma(BRAKE_TIME); // ... e fermati!
    cputs("bordo");
    vai_indietro(); // Torna indietro ... 
    wait_event(&sensor_release_wakeup,0); // ... finche' non sei lontano dal bordo ...
    vai_indietro(); // ... e poi ancora per un po'.
    sleep(1);
    gira_a_destra(TURN_TIME); // Cambia direzione.
    vai_diritto(); // Avanti ...
  }
}

int trova_scuro(int argc, char **argv){
  ds_active(&SENS_LIGHT); // attiva il sensore di luce
  msleep(50);
  wait_event(&light_wakeup,0); // ... finche' non trovi scuro
  ferma(BRAKE_TIME); // ... e fermati!
  ds_passive(&SENS_LIGHT); // disattiva il sensore di luce
  FINE = 1;
  return 0;
}