/*written by Mike Bohan*/
#use "alc_cbus.icb"

#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



/*
    This function queries the slave module with the danger command.  
    The return value is a byte with the each bit indicating whether that sonar
    value is below the safe threshold
*/

int inDanger()
{
    is_cbus_cmd = 1; // reset flag (this value will be set to one after a byte of data is received)
    cbus_send_cmd(SONAR_HB); //         while(is_cbus_cmd == 1); // wait for compass to send data byte back
    while(is_cbus_cmd == 1);
    return cbus_byte;
}

int querySonar(int i)
/*
get the value of a specific sonar.
input is value 0 to 7
output is one byte version of sonar value
*/
{
    int high_byte;
    is_cbus_cmd = 1; // reset flag (this value will be set to one after a byte of data is received)
    cbus_send_cmd(SONAR_0 + i); 
    while(is_cbus_cmd == 1); // wait for sonar HB to send data byte back
    high_byte = cbus_byte << 8;
    is_cbus_cmd = 1;
    while(is_cbus_cmd == 1); // wait for sonar HB to send data byte back
    return high_byte | cbus_byte;
}




/* this function takes a byte returned by the sonar subsystem (in proto 3) and
   returns whether the sonar num provided is 'triggered'
*/
int decodeBitCode(int bitcode, int sonarnum)
{
    int x = 1;
    
    x = x << sonarnum-1;
    // printf("%d %d %d\n", bitcode, sonarnum, (x & bitcode));
    if(x & bitcode) 
      return 1;
    
    return 0;              
}    


int convertToIndex(int prot_cmd)
{
    int i;
    int x;
    
    for(i=0;i<8;i++) {
        x = 1;
        x = x << i;
        printf("%d\n", (prot_cmd & x));
        if((prot_cmd & x)== 1)
          return i+1;
    }    
    
    return 0;
}