byte CMDNunchukInit[] = {0x03, 0xA4, 0x40, 0x00};
byte CMDNunchukWrite[] = {0x02, 0xA4, 0x00};
byte CMDNunchukRead[] = {0x01, 0xA4};

// byte nbytes;    //NXC
byte outbuf[6];
int outbuf1[6];   //ubyte workaround for RobotC

//  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;
}

// port is the NXT port, i2c_cmd is the nunchuk internal address register
// at i2c_cmd=0...5 are 6 Byte of sensor values
// at i2c_cmd=0x20 ... 0x2F  you get 16 Byte of calibration data

//sub NunchukGetParamter(byte port, byte i2c_cmd)    //NXC
sub NunchukGetParamter(tSensors port, byte i2c_cmd)  //RobotC
{
  // Initialize the Nunchuk
  //SetSensorLowspeed(port);  //NQC
   SensorType[port] = sensorI2CCustom;  //RobotC

  //LowspeedWrite(port, 0, CMDNunchukInit); //NXC ask for 0 bytes
   sendI2CMsg(port, CMDNunchukInit[0], 0); //RobotC

  //while(LowspeedStatus(port, nbytes)>0);  //NQC
  while (nI2CStatus[port] == STAT_COMM_PENDING) ; // RobotC Wait for I2C bus to be ready

  //CMDNunchukWrite[1] =i2c_cmd;   //NXC at 0x00: 6 Bytes of sensor values
  CMDNunchukWrite[2] =i2c_cmd;     //RobotC

  //LowspeedWrite(port, 0, CMDNunchukWrite);   //NXC write Nunchuk register address
   sendI2CMsg(port, CMDNunchukWrite[0], 0);     //RobotC

  //while(LowspeedStatus(port, nbytes)>0);        //NXC
  while (nI2CStatus[port] == STAT_COMM_PENDING) ; // RobotC Wait for I2C bus to be ready

  byte count = 6;
  //LowspeedWrite(port, count, CMDNunchukRead); //NXC: ask for count bytes
   sendI2CMsg(port, CMDNunchukRead[0], count);   //RobotC

  //while(LowspeedStatus(port, nbytes)>0);        //NXC
  while (nI2CStatus[port] == STAT_COMM_PENDING) ; // Wait for I2C bus to be ready

  //if(nbytes == count){                  //NXC
  if(nI2CBytesReady[port] == count){      //RobotC

    //LowspeedRead(port, count, outbuf)  //NXC read data from buffer
      readI2CReply(port, outbuf[0], 6);    //RobotC

    for(int i=0; i<count; i++ ){
       outbuf1[i]=ubyteToInt(outbuf[i]);   //ubyte workaround for RobotC
      outbuf1[i]=(outbuf1[i]^0x17) + 0x17;
    }
  }
  else {
    //ArrayInit(outbuf,0,0);    //NXC: error
     memset(outbuf, 0, 0);       //RobotC
  }
}

task main()
{
  while (true)
  {
    NunchukGetParamter(S1, 0x00);          //see comment for i2c_cmd

    //if (ArrayLen(outbuf)==6) {     //NXC
    if (sizeof(outbuf)==6)    {      //RobotC

       //string Nunchuk="aXaYgXgYgZCZ";     //NXC
       char Nunchuk[16];                    //RobotC
       strcpy(Nunchuk, "aXaYgXgYgZCZ");     //RobotC

       if ((outbuf1[5]&0x01)!=0) {Nunchuk[11]=122;}  //Show Z-Button
       if ((outbuf1[5]&0x02)!=0) {Nunchuk[10]=99;}   //Show C-Button

       // ClearScreen();  //NXC
       eraseDisplay();   //RobotC

       // for(int i=0; i<ArrayLen(outbuf); i++ ) {  //NXC
       for(int i=0; i<6; i++ ) {  //RobotC

         //TextOut(15*i, LCD_LINE3, SubStr(Nunchuk, 2*i, 2));   //NXC
         nxtDisplayStringAt(15*i,50,"%c", Nunchuk[2*i]);         //RobotC
         nxtDisplayStringAt(15*i+5,50,"%c", Nunchuk[2*i+1]);       //RobotC

         //TextOut(15*i, LCD_LINE5, FormatNum("%02X", outbuf[i]));     //NXC
         nxtDisplayStringAt(15*i,40,"%02X", outbuf1[i]);               //RobotC
       }

       // CircleOut((outbuf[0]-0x1E)/2, (outbuf[1]-0x1E)/3, 3);    //NXC analog stick
       nxtDrawCircle((outbuf1[0]-0x1E)/2, (outbuf1[1]-0x1E)/3, 3);   //RobotC

       // RectOut((outbuf[2]-0x46), (outbuf[3]-0x46)/2, 6, 6);    //NXC acceleration value
       nxtDrawCircle((outbuf1[2]-0x46), (outbuf1[3]-0x46)/2, 6);
    }
    // Wait(100);  //NXC
    wait1Msec(100);    //RobotC
  }
}