/*
 *  Program to read values from RCX and display on NXT screen.
 */
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
/*
 * run this program (enclosed between +++ comments) on RCX.
 * rest of the program is run on NXT.
//
// a program to transmit incrementing number 
// on IR interface of RCX every 1 second.
//
 
 task main()
 {
   int i;
   SetTxPower(TX_POWER_HI);
   
   while (true) {
     for ( i = 0; i < 25; i++ ) {
         SendMessage(i);
         Wait (100); // wait 1 second.
     }
   }
 }
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/


#define PORT S1

const byte NRLinkID = 0x02;
const byte NRLinkDataBytes = 0x40;
const byte NRLinkCommandReg = 0x41;
const byte NRLinkReadResult = 0x42;
const byte NRLinkWriteData = 0x42;

const byte NRLinkDefault = 0x44;
const byte NRLinkFlush = 0x46;
const byte NRLinkHighSpeed = 0x48;
const byte NRLinkLongRange = 0x4C;
const byte NRLinkShortRange = 0x53;
const byte NRLinkSetADPAON = 0x4E;
const byte NRLinkSETADPAOFF = 0x4F;
const byte NRLinkTxUnassembled = 0x55;


const byte NRLinkSelectRCX = 0x58;
const byte NRLinkSelectTRAIN = 0x54;
const byte NRLinkSelectPF = 0x50;

string format_hex ( int loop ) {
  int j;
  string s;
  int b = 0xF0;
  int a = 0x0F;

  j = (loop&b)>>4;

  s = "";
  switch ( j ) {
  case 1:
      s += "1";
      break;
  case 2:
      s += "2";
      break;
  case 3:
      s += "3";
      break;
  case 4:
      s += "4";
      break;
  case 5:
      s += "5";
      break;
  case 6:
      s += "6";
      break;
  case 7:
      s += "7";
      break;
  case 8:
      s += "8";
      break;
  case 9:
      s += "9";
      break;
  case 10:
      s += "A";
      break;
  case 11:
      s += "B";
      break;
  case 12:
      s += "C";
      break;
  case 13:
      s += "D";
      break;
  case 14:
      s += "E";
      break;
  case 15:
      s += "F";
      break;
  case 16:
      s += "0";
      break;
  case 0:
      s += "0";
      break;
  }

  j = loop&a;

  switch ( j ) {
  case 1:
      s += "1";
      break;
  case 2:
      s += "2";
      break;
  case 3:
      s += "3";
      break;
  case 4:
      s += "4";
      break;
  case 5:
      s += "5";
      break;
  case 6:
      s += "6";
      break;
  case 7:
      s += "7";
      break;
  case 8:
      s += "8";
      break;
  case 9:
      s += "9";
      break;
  case 10:
      s += "A";
      break;
  case 11:
      s += "B";
      break;
  case 12:
      s += "C";
      break;
  case 13:
      s += "D";
      break;
  case 14:
      s += "E";
      break;
  case 15:
      s += "F";
      break;
  case 16:
      s += "0";
      break;
  case 0:
      s += "0";
      break;
  } 
  return (s);
}

string format_bin ( int loop ) {

  string s;
  int j;
  int b = 0x80;
  s = "";
  for ( j = 0; j < 8; j++) {
    if ( loop&b ) {
      s += NumToStr(1);
    } else {
      s += NumToStr(0);
    }
    b = b>>1;
  }
  return (s);
}

void NRLinkCommand(byte NRLinkCommand)
{

   byte NRLinkMsg[];
   ArrayBuild(NRLinkMsg,
              NRLinkID,
              NRLinkCommandReg,
              NRLinkCommand);

   byte nByteReady = 0;
   while (I2CStatus(PORT, nByteReady) ==  STAT_COMM_PENDING)
   {
   // Wait for I2C bus to be ready
   }
   // when the I2C bus is ready, send the message you built
   I2CWrite(PORT, 0, NRLinkMsg);
}


string i2cReadString(byte prt, byte adr, byte reg, byte cnt)
{
  byte outbuf[];
  byte cmdbuf[];
  string temp = "";
  int loop;

  ArrayBuild(cmdbuf, adr, reg);
  byte nByteReady = 0;

  loop = STAT_COMM_PENDING;
  while ( loop == STAT_COMM_PENDING ) {
    loop = I2CStatus(prt, nByteReady);
  }
  if ( loop != NO_ERR ) {
    string msg2;
    msg2 = "B.1> Error:";
    msg2 += NumToStr(loop);
    msg2 += "  ";
    TextOut(0, LCD_LINE8, msg2, false);
    return temp;
  }

  if(I2CBytes(prt, cmdbuf, cnt, outbuf))
  {
    temp = ByteArrayToStr(outbuf);
  }
  return temp;
}

void ShowSensorInfo(byte prt)
{
  ClearScreen();
  TextOut(0, LCD_LINE1, i2cReadString(prt, NRLinkID, 0x00, 8));
  TextOut(35, LCD_LINE1, i2cReadString(prt, NRLinkID, 0x08, 8));
  TextOut(0, LCD_LINE2, i2cReadString(prt, NRLinkID, 0x10, 8));
}

 task main()           // main task:
{
  string st;
  string msg;
  byte buf[30];
  int result;

  string x;
  byte outbuf[30];
  byte cmdbuf[30];
  byte cnt, cnt2;
  int i, loop, retval, n;
  byte nByteReady;


  SetSensorLowspeed(PORT);
  ShowSensorInfo(PORT);
  TextOut(0, LCD_LINE3, "Press Orange btn");
  until(ButtonPressed(BTNCENTER, true));

  NRLinkCommand(NRLinkFlush);
  NRLinkCommand(NRLinkDefault);
  NRLinkCommand(NRLinkLongRange);
  NRLinkCommand(NRLinkSelectPF);

  loop = 0;
  while(true)
  {
    /*
    * fetch how many bytes are available to read
    */
    ArrayBuild(cmdbuf, NRLinkID, 0x40);
    nByteReady = 0;
		n = STAT_COMM_PENDING;
		while ( n == STAT_COMM_PENDING) {
			n = I2CStatus(PORT, nByteReady);
		}
		if ( n != NO_ERR ) {
			string msg2;
			msg2 = "A.0> Error:";
			msg2 += NumToStr(n);
			msg2 += "  ";
			TextOut(0, LCD_LINE8, msg2, false);
			continue;
		}
    cnt = 1;
		retval = I2CBytes(PORT, cmdbuf, cnt, buf);
    if( retval == TRUE ) {
      result = buf[0];
    } else {
			string msg2;
			msg2 = "A.1> Error: read";
			msg2 += "  ";
			TextOut(0, LCD_LINE8, msg2, false);
			continue;
		}
    cnt = result;

    /*
     * fetch all available bytes
     */
    ArrayBuild(cmdbuf, NRLinkID, 0x42);
    nByteReady = 0;
		n = STAT_COMM_PENDING;
		while ( n == STAT_COMM_PENDING) {
			n = I2CStatus(PORT, nByteReady);
		}
		if ( n != NO_ERR ) {
			string msg2;
			msg2 = "A.2> Error:";
			msg2 += NumToStr(n);
			msg2 += "  ";
			TextOut(0, LCD_LINE8, msg2, false);
			continue;
		}

		retval = I2CBytes(PORT, cmdbuf, cnt, buf);
    if( retval == TRUE ) {
      st = "loop: ";
      st += NumToStr(loop);
      st += " pkt: ";
      st += NumToStr(cnt);
      st += "     ";
      TextOut(0, LCD_LINE4, st);

/*
      st = "                     ";
      TextOut(0, LCD_LINE5, st, false);
      TextOut(0, LCD_LINE6, st, false);
      st = "";
      if ( cnt <=4 ) {
        cnt2 = cnt;
      } else {
        cnt2 = 4;
      }
      st = "0-3:";
      for (i =0; i < cnt2; i++) {
        result = buf[i];
        st += format_hex(result);
        st += " ";
      }
      TextOut(0, LCD_LINE5, st, false);

      if ( cnt > 4 ) {
        st = "4-7:";
        for (i =4; i < cnt; i++) {
          result = buf[i];
          st += format_hex(result);
          st += " ";
        }
        TextOut(0, LCD_LINE6, st, false);
      }

*/
      if ( buf[0] == 0x55 &&
			     buf[1] == 0xFF &&
					 buf[2] == 0x00 &&
					 buf[3] == 0xF7 &&
					 buf[4] == 0x08 &&
					 cnt > 5) {
        st = "RCX data: ";
        st += NumToStr(buf[5]);
        st += "      ";
//        PlayTone(buf[5]*25, buf[5]*25);
      } else {
        st = "                     ";
//        PlayTone(500, 500);
      }
      TextOut(0, LCD_LINE7, st, false);
    } else {
			string msg2;
			msg2 = "A.4> Error: read";
			msg2 += "  ";
			TextOut(0, LCD_LINE8, msg2, false);
			continue;
		}

     Wait(1000);
     loop++;
  }
}


/*
 * You can find RCX internals, and protocol packet information here:
 * http://graphics.stanford.edu/~kekoa/rcx/
 *
 */