//*!!Sensor,    S1,               NRLINK, sensorI2CCustomStd9V,      ,           !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
const tSensors NRLINK               = (tSensors) S1;   //sensorI2CCustomStd9V //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

/*
*
* Dump the contents of the EEPROM in the mindsensors NRLINK
* sensor and record them to a data log file and print on the screen
*
* Use this program to debug writing macros into the NRLink-Nx device
*
* Modified by Mark Crosbie mark@mastincrosbie.com  1 April 2008
* To install macros that power both motors simultaneously
* For more information visit http://markclego.mastincrosbie.com
*
*
* Original code written by Dr. Nitin Patil
* Copyright (c) mindsensors.com 2006, 2007
* for more info visit www.mindsensors.com
*
*/

//definations for NRLink

const ubyte NRLinkID = 0x02;
const ubyte NRLinkDataBytes = 0x40;
const ubyte NRLinkCommandReg = 0x41;
const ubyte NRLinkReadResult = 0x42;
const ubyte NRLinkWriteData = 0x42;
const tSensors NRLinkPort = NRLINK; // Connect NRLink sensor to this port!!


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

const ubyte NRLinkSelectPF = 0x50;

/////////////////////////////////////////////////////////////////////////////
//
//     send Command to NrLink interface
//
/////////////////////////////////////////////////////////////////////////////
void NRLinkCommand(byte  NRLinkCommand)
{

	byte NRLinkMsg[5];
	const byte MsgSize         = 0;
	const byte Address         = 1;
	const byte CommandAddress  = 2;
	const byte Command         = 3;


	// Build the I2C message
	NRLinkMsg[MsgSize]        = 3;
	NRLinkMsg[Address]        = NRLinkID;
	NRLinkMsg[CommandAddress] = NRLinkCommandReg  ;
	NRLinkMsg[Command]        = NRLinkCommand;

	while (nI2CStatus[NRLinkPort] == STAT_COMM_PENDING)
	{
	  // Wait for I2C bus to be ready
	}
	// when the I2C bus is ready, send the message you built
	sendI2CMsg(NRLinkPort, NRLinkMsg[0], 0);
}

// Print the contents of the NRLink RAM starting and
// ending at the addresses given
//
// Each line is printed and the display wraps around
// every 8 lines
void readMacros(ubyte startAddr, ubyte endAddr) {

	ubyte i, line;
	ubyte addr;
  const int kReplyLength = 1;
  ubyte nReplyBytes[kReplyLength];

  string s1, s2;

  TI2CStatus err;

  typedef struct{
    byte nMsgSize;
    byte nDeviceAddress;
    byte nLocationPtr;
  } TI2C_Output;

  TI2C_Output sOutput;
  sOutput.nMsgSize = 2;
  sOutput.nDeviceAddress = 0x02;
  sOutput.nLocationPtr = 0;

	eraseDisplay();

	line=0;
	addr = startAddr;
  for(i=(int)startAddr; i < (int)endAddr; i++) {

	  nI2CBytesReady[NRLinkPort]=0;
	  err = NO_ERR;
	  do {
	  	err = nI2CStatus[NRLinkPort];
	  	wait1Msec(2);
	  }	while(err == STAT_COMM_PENDING);

	  sOutput.nLocationPtr = addr;
    addr++;

	  sendI2CMsg(NRLinkPort, sOutput.nMsgSize, kReplyLength);

	  err = NO_ERR;
	  do {
	  	err = nI2CStatus[NRLinkPort];
	  	wait1Msec(2);
	  }	while(err == STAT_COMM_PENDING);

  	err = nI2CStatus[NRLinkPort];
    if (err == NO_ERR){
      readI2CReply(NRLinkPort, nReplyBytes[0], kReplyLength);

      StringFormat(s1, "%x: %d", i, nReplyBytes[0]);
      //StringFormat(s2, "%2x %2x", nReplyBytes[1], nReplyBytes[2]);

      s2 = "";
      s2 += nReplyBytes[0];

      nxtDisplayTextLine(line%8, "%s %s", s1, s2);
  	  wait10Msec(100);
	  } else {
	  	nxtDisplayTextLine(line%8, "%2x: err %d", i, err);
  	  wait10Msec(100);
  	}
  	line++;
  }
}

// Write the contents of the NRLink RAM into a file on the NXT that
// can be downloaded from the RobotC IDE onto your PC
//
// File is named testFile.dat
//
void dumpNRLinkRAM() {

	int i;
	byte addr;
  const int kReplyLength = 1;
  ubyte nReplyBytes[kReplyLength];

  const string sFileName = "testFile.dat";
  TFileIOResult nIoResult;
  TFileHandle hFileHandle;
  int nFileSize 					= 256;

  TI2CStatus err;

  typedef struct{
    byte nMsgSize;
    byte nDeviceAddress;
    byte nLocationPtr;
  } TI2C_Output;

  TI2C_Output sOutput;
  sOutput.nMsgSize = 2;
  sOutput.nDeviceAddress = 0x02;
  sOutput.nLocationPtr = 0;

  Delete(sFileName, nIoResult);
	hFileHandle = 0;
  OpenWrite(hFileHandle, nIoResult, sFileName, nFileSize);

  nxtDisplayTextLine(2, "Dumping NRLink");

  addr = 0;
  for(i=0x00; i <= 0xff; i++) {

	  nI2CBytesReady[NRLinkPort]=0;
	  err = NO_ERR;
	  do {
	  	err = nI2CStatus[NRLinkPort];
	  	wait1Msec(2);
	  }	while(err == STAT_COMM_PENDING);

	  sOutput.nLocationPtr = addr;
    addr++;

	  sendI2CMsg(NRLinkPort, sOutput.nMsgSize, kReplyLength);

	  err = NO_ERR;
	  do {
	  	err = nI2CStatus[NRLinkPort];
	  	wait1Msec(2);
	  }	while(err == STAT_COMM_PENDING);

  	err = nI2CStatus[NRLinkPort];
    if (err == NO_ERR){
      readI2CReply(NRLinkPort, nReplyBytes[0], kReplyLength);
      WriteByte(hFileHandle, nIoResult, nReplyBytes[0]);
	  } else {
	  	WriteByte(hFileHandle, nIoResult, 0xff);
  	}
  }

  Close(hFileHandle, nIoResult);

  nxtDisplayTextLine(3, "Done");
}

/////////////////////////////////////////////////////////////////////////////
//
// Run some commands and macro to control PF Motors using NRLink.
//
/////////////////////////////////////////////////////////////////////////////

task main()
{

  nI2CBytesReady[NRLinkPort] = 0;
  SensorType[NRLinkPort] = sensorI2CCustom9V;  // Note, use this, NOT CustomFast9V!!
  NRLinkCommand(NRLinkFlush);
  NRLinkCommand(NRLinkDefault);
  // NRLinkCommand(NRLinkLongRange);
  NRLinkCommand(NRLinkSelectPF);

  eraseDisplay();

  dumpNRLinkRAM();

  wait10Msec(100);

  // print the contents of the NRlink RAM starting and ending at these addresses
  readMacros(0x0, 0x17);

	wait10Msec(1000);
}