#define SONAR_HB 1
#define SONAR_0 2
#define SONAR_1 3
#define SONAR_2 4
#define SONAR_3 5
#define SONAR_4 6
#define SONAR_5 7
#define SONAR_6 8
#define SONAR_7 9

#define SafeDist 500


#use "alc_cbus.icb"
// network assembly routines
// plug network into digital input 9

#use "alc_cmps03.ic"
// compass stuff
/*
// plug in compass into digital input 7
int compass();
*/
#use "timer_lib.ic"
#use "movement_lib.ic"
// simple movement commands
/* MOVEMENT PROTOTYPES
int forward(int speed);
int back(int speed);
int right(int speed);
int rev_right(int speed);
int left(int speed);
int rev_left(int speed);
int stop();
void movement_init();
*/

#use "sonar_lib.ic"
// sonar functions
/* SONAR PROTOTYPES
int inDanger();
int querySonar(int i);
*/

//#use "cd_lib.ic"
//CD Player functions
/*CD PLAYER PROTOTYPES
void power_on()
void power_off()
void cd_stop()
void play()
void pause()
void fast_forward(int track_number)
void backwards(int track_number)
*/


void main()
{
    int s0, s1, s2, s3, s4, s5, s6, s7;
    movement_init();
    install_PAI_interrupt(0);   // install interrupt into vector table
    stop();
    start_press();              // wait for all connected Handyboards to be turned on before pressing start
    listen_to_cbus(0);          // this enables the interrupt to happen 
    
    wander();
}

void wander_avoid(){
    
    int r=2,d;
    Reset_Timer(1);
    
    while(1)
      {
        d = inDanger(); /* This will poll the sonar board for danger byte */
        
        printf("front=%d  back=%d  r=%d\n",d & 0x0D, d & 0xC2,r);
        msleep(100L);
        
        // This is the random wander part
        if(((d & 0x01) == 0) && (Get_Timer(2) > 2000L)) // the second timer ensures that it atleast travels for two second before picking a new direction
          {
            if(Get_Timer(1) > 2000L){ r = random(3); Reset_Timer(1);}
            if(r == 0) left(15);
            else if(r == 1) right(15);
              else forward(15);
        }
        
        
        else
          {
            // This is the avoidance part
            if((d & 0xC3) != 0) stop();
            else 
              {
                Reset_Timer(2);
                if( (d & 0x04) != 0 ) /*right(15);*/rev_left(15);
                else if( (d & 0x08) != 0) /*left(15);*/rev_right(15);
                  else if( (d & 0x01) != 0) back(15);
            }
        }
        
    }
}


#define frontM ((d & 0x01)!=0)
#define leftM ((d & 0x04)!=0)
#define rightM ((d & 0x08)!=0)
#define backM ((d & 0x02)!=0)

#define motorpower 35
void wander()
{
    int d,r;
    Reset_Timer(1);
    while(1){
        d = inDanger();
        
        if(frontM || (leftM && rightM)) avoid();
        else if(Get_Timer(1) > 2000L)
            {
              if(leftM) right(motorpower);
              else if(rightM) left(motorpower);
                else {
                    r = random(3);
                    if(r==0) left(motorpower);
                    else if(r == 1) right(motorpower);
                      else forward(motorpower);
                    
                }
              Reset_Timer(1);
          }
          printf("norm :%b \n",d );          
    }
    
    
    
}

void avoid()
{
    int d;
    Reset_Timer(2);
    while(Get_Timer(2) < 2000L)
      {
        d = inDanger();           printf("avoid:%b \n",d );         msleep(100L);
        
        if(!backM) back(motorpower);
        else stop();   
    }   
}