#pragma config(Sensor, S1,     MD8,                 sensorNxtgSONARCm)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

byte MD8Read[] = {0x01, 0x70};

byte outbuf[1];
byte count = 1;
int d[4],e[4];



//  This function allows conversion of an unsigned byte to a signed int
//  This is a workaround for RobotC
int ubyteToInt(ubyte _byte) {
    int _ret = 0;
    _ret = (_byte & 0x80) ? (_byte & 0x7F) + 0x80 : _byte;
    return _ret;
}

int MD8_Read(tSensors port)
{
   int out;
  while (nI2CStatus[port] == STAT_COMM_PENDING); // Wait for I2C bus to be ready
  sendI2CMsg(port, MD8Read[0], count);

  while (nI2CStatus[port] == STAT_COMM_PENDING); // Wait for I2C bus to be ready
  if(nI2CBytesReady[port] == count)
  {
     readI2CReply(port, outbuf[0], 1);
    out=ubyteToInt(outbuf[0]);
  }
  else {
      memset(outbuf, 0, 0);       //RobotC
  }
  return out;
}

void number2bool(int out)
{
    d[3]=(out&0x80)/128;
    d[0]=(out&0x40)/64;
    d[1]=(out&0x20)/32;
    d[2]=(out&0x10)/16;

    e[3]=(out&0x01);
    e[0]=(out&0x02)/2;
    e[1]=(out&0x04)/4;
    e[2]=(out&0x08)/8;
}

task main()
{
  while (true)
  {
    number2bool( MD8_Read(MD8) );     //Get value

    nxtDisplayTextLine(2,"E1:%d,E2:%d",e[0],e[1]);
    nxtDisplayTextLine(3,"E3:%d,E4:%d",e[2],e[3]);

    nxtDisplayTextLine(5,"D1:%d,D2:%d",d[0],d[1]);
    nxtDisplayTextLine(6,"D3:%d,D4:%d",d[2],d[3]);

    wait1Msec(100);

  }
}