/*  sonar protocol specifications:
         1. gain control of bus
         2. next byte indicates desired query
         3. Send back x bytes depending on desired request

         First byte: (prot_cmd)
            3: =  inDanger() (since a byte with only 1 bit active means a specific sonar reading is requested) 
                   00000011 has > 1 bit high, so it is acceptable for a value
*/


#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



#define SAFEDIST 900

int s[8];
int ringbuf[8][3];
int ringpos;

int main(void)
{
    int sl_pid;
    
    ringpos = 0;
    
    install_PAI_interrupt(0);   // install interrupt into vector table
    sl_pid = start_process(sonarLoop());
    
    start_press();                   // wait for all connected Handyboards to be turned on before pressing start
    listen_to_cbus(0);            // this enables the interrupt to happen 
    
    while(1)
      {
        
        if(is_cbus_cmd)
          {
            hog_processor();
            if( cbus_byte == SONAR_HB )
              {
                cbus_send_data(dangerByte());
            } else if( cbus_byte == SONAR_0 )
                {
                  cbus_send_data(s[0] >> 8);
                  msleep(5L);
                  cbus_send_data(s[0] & 0xFF);
              } else if( cbus_byte == SONAR_1 )
                  {
                    cbus_send_data(s[1] >> 8);
                    msleep(5L);
                    cbus_send_data(s[1] & 0xFF);
                } else if( cbus_byte == SONAR_2 )
                    {
                      cbus_send_data(s[2] >> 8);
                      msleep(5L);
                      cbus_send_data(s[2] & 0xFF);
                  } else if( cbus_byte == SONAR_3 )
                      {
                        cbus_send_data(s[3] >> 8);
                        msleep(5L);
                        cbus_send_data(s[3] & 0xFF);
                    } else if( cbus_byte == SONAR_4 )
                        {
                          cbus_send_data(s[4] >> 8);
                          msleep(5L);
                          cbus_send_data(s[4] & 0xFF);
                      } else if( cbus_byte == SONAR_5 )
                          {
                            cbus_send_data(s[5] >> 8);
                            msleep(5L);
                            cbus_send_data(s[5] & 0xFF);
                        } else if( cbus_byte == SONAR_6 )
                            {
                              cbus_send_data(s[6] >> 8);
                              msleep(5L);
                              cbus_send_data(s[6] & 0xFF);
                          } else if( cbus_byte == SONAR_7 )
                              {
                                cbus_send_data(s[7] >> 8);
                                msleep(5L);
                                cbus_send_data(s[7] & 0xFF);
                            } 
                            
                            
                            is_cbus_cmd = 0;
            defer();
        }
    }    
    return 0;
}

void averageBuffer(int num)
{
    int p, q;
    long sum;
    
    sum = 0L;
    for(q=0;q<3;q++) 
      sum = sum + (long)ringbuf[num][q];
    sum = (long)(((float)sum) / 3.0);
    s[num]= (int)sum;        
}


int dangerByte()
{
    int i, shift;
    int bitcode = 0;
    for(i=0;i<8;i++) 
      {
        averageBuffer(i);
        if((i == 1)||(i == 6) || (i ==7))
          {
            if(s[i] < SAFEDIST - 300) {
                shift = 1;
                shift = shift << i;
                bitcode = bitcode | shift;                      
            }            
        }else if((i == 2) || (i ==3))
            {
              if(s[i] < SAFEDIST + 300) {
                  shift = 1;
                  shift = shift << i;
                  bitcode = bitcode | shift;                      
              }            
          }
          else{
              
              if(s[i] < SAFEDIST) {
                  shift = 1;
                  shift = shift << i;
                  bitcode = bitcode | shift;                      
              }
          }
        
    }
    return bitcode;
}

int sonar_testing()
{    // test thingy
    start_process(sonarLoop());
    while(1){
        printf("%b\n", dangerByte());
        msleep(100L);
    }
    
    
}






int soloBit(int prot_cmd)
{
    int shift;    
    int i;
    int x = 1;
    int count = 0;
    
    for(i=0;i<8;i++) {
        if(prot_cmd & x)
          count++;
        if(count > 1)
          return 0;
        prot_cmd >> 1;
    }
    return 1;
}

void sonarLoop()
{
    int i;
//    long m;
  //  long n;
    
    
    while(1) {
  //      m = mseconds();
        for(i=1;i<9;i++) {
            callSonar(i);
            ringbuf[i-1][ringpos]=s[i-1];
        }
        ringpos = (ringpos + 1) % 3;
//        n = mseconds() - m;
        //  printf("lag: %d   %d\n", (int)n,s[6] );
    }
}

int sonar1()
{
    clear_digital_out(2);
    clear_digital_out(4);
    clear_digital_out(6);
    s[0] = sonar();
    //msleep(40L);
}

int sonar2()
{
    set_digital_out(2);
    clear_digital_out(4);
    clear_digital_out(6);
    s[1] = sonar();
    //    msleep(40L);    
}

int sonar3()
{
    clear_digital_out(2);
    set_digital_out(4);
    clear_digital_out(6);
    s[2] = sonar();
    //    msleep(40L);    
}

int sonar4()
{
    set_digital_out(2);
    set_digital_out(4);
    clear_digital_out(6);
    s[3] = sonar();
    //    msleep(40L);        
}

int sonar5()
{
    clear_digital_out(2);
    clear_digital_out(4);
    set_digital_out(6);
    s[4] = sonar();
    //    msleep(40L);        
}

int sonar6()
{
    set_digital_out(2);
    clear_digital_out(4);
    set_digital_out(6);
    s[5] = sonar();
    //    msleep(40L);    
}

int sonar7()
{
    clear_digital_out(2);
    set_digital_out(4);
    set_digital_out(6);
    s[6] = sonar();
    //    msleep(40L);    
}

int sonar8()
{
    set_digital_out(2);
    set_digital_out(4);
    set_digital_out(6);
    s[7] = sonar();
    //    msleep(40L);        
}

void callSonar(int i)
{
    if(i==1) 
      sonar1();
    if(i==2) 
      sonar2();
    if(i==3) 
      sonar3();                              
    if(i==4) 
      sonar4();                                
    if(i==5) 
      sonar5();    
    if(i==6) 
      sonar6();            
    if(i==7) 
      sonar7();                               
    if(i==8) 
      sonar8();  
}
/*
     This function takes a byte containing one active sonar value and gets the equivalent sonar index value.
     This is equal to log(2, val)

     Returns 0 on failure
*/

int convertToIndex(int prot_cmd)
{
    int i;
    int x;
    
    for(i=0;i<8;i++) {
        x = 1;
        x = x << i;
        if((prot_cmd & x))
          return i+1;
    }    
    
    return 0;
}