/***************************************************************
A simple progam controls PF in NXC, based on Dr. Nitin's RobortC code
Tested with NRLink sensor and BullDozer

Author: Ziming Wang

***************************************************************/

#define PORT S3

//definations for NRLink

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;


const byte NRLinkMacro = 0x52;
const byte Macro_Short_range = 0x01;
const byte Macro_Long_Range = 0x04;


const byte Motor_Ch1_A_Float = 0x50;
const byte Motor_Ch1_A_FWD = 0x53;
const byte Motor_Ch1_A_REV = 0x56;
const byte Motor_Ch1_A_Brake = 0x59;

const byte Motor_Ch1_B_Float = 0x5C;
const byte Motor_Ch1_B_FWD = 0x5F;
const byte Motor_Ch1_B_REV = 0x62;
const byte Motor_Ch1_B_Brake = 0x65;

const byte Motor_Ch2_A_Float = 0x68;
const byte Motor_Ch2_A_FWD = 0x6B;
const byte Motor_Ch2_A_REV = 0x6E;
const byte Motor_Ch2_A_Brake = 0x71;

const byte Motor_Ch2_B_Float = 0x74;
const byte Motor_Ch2_B_FWD = 0x77;
const byte Motor_Ch2_B_REV = 0x7A;
const byte Motor_Ch2_B_Brake = 0x7D;

const byte Motor_Ch3_A_Float = 0x80;
const byte Motor_Ch3_A_FWD = 0x83;
const byte Motor_Ch3_A_REV = 0x86;
const byte Motor_Ch3_A_Brake = 0x89;

const byte Motor_Ch3_B_Float = 0x8C;
const byte Motor_Ch3_B_FWD = 0x8F;
const byte Motor_Ch3_B_REV = 0x92;
const byte Motor_Ch3_B_Brake = 0x95;

const byte Motor_Ch4_A_Float = 0x98;
const byte Motor_Ch4_A_FWD = 0x9B;
const byte Motor_Ch4_A_REV = 0x9E;
const byte Motor_Ch4_A_Brake = 0xA1;

const byte Motor_Ch4_B_Float = 0xA4;
const byte Motor_Ch4_B_FWD = 0xA7;
const byte Motor_Ch4_B_REV = 0xAA;
const byte Motor_Ch4_B_Brake = 0xAD;


/////////////////////////////////////////////////////////////////////////////
//
// send macro and run it from NRLink interface
//
//
/////////////////////////////////////////////////////////////////////////////

void NRLinkRunMacro(byte NRLinkMacroAdd)
{

   byte NRLinkMsg[];
   ArrayBuild(NRLinkMsg,
              NRLinkID,
              NRLinkCommandReg,
              NRLinkMacro,
              NRLinkMacroAdd);
   

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

/////////////////////////////////////////////////////////////////////////////
//
// send Command to NrLink interface
//
//
/////////////////////////////////////////////////////////////////////////////

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



// read data from sensor register(s)
int i2cread(byte prt, byte adr, byte reg, byte cnt)
{
  int result = -1;             // default: error value
  byte outbuf[];               // here we will get data
  byte cmdbuf[];               // register number holder

  ArrayBuild(cmdbuf, adr, reg);
  if(I2CBytes(prt, cmdbuf, cnt, outbuf))
  {
    result = outbuf[0];       // read value

    if(cnt==2)
      result = result + outbuf[1]*256;
// if 2 registers (16 bit), then add the MSB part 
  }

  return result;              // returns -1 if I2CBytes failed
}

string i2cReadString(byte prt, byte adr, byte reg, byte cnt)
{
  byte outbuf[];
  byte cmdbuf[];
  string temp = "";
  
  ArrayBuild(cmdbuf, adr, reg);
  byte nByteReady = 0;
  while (I2CStatus(PORT, nByteReady) ==  STAT_COMM_PENDING)
  {
  // Wait for I2C bus to be ready
  }
  
  if(I2CBytes(prt, cmdbuf, cnt, outbuf))
  {
    temp = ByteArrayToStr(outbuf);
  }
  return temp;
}


//Print sensor information from its register
void ShowSensorInfo(byte prt)
{
  ClearScreen();
  TextOut(0, LCD_LINE1, i2cReadString(prt, NRLinkID, 0x00, 8));
  TextOut(0, LCD_LINE2, i2cReadString(prt, NRLinkID, 0x08, 8));
  TextOut(0, LCD_LINE3, i2cReadString(prt, NRLinkID, 0x10, 8));
}


task main()           // main task:
{
  SetSensorLowspeed(PORT);
  ShowSensorInfo(PORT);
  until(ButtonPressed(BTNCENTER, true));
  TextOut(0, LCD_LINE4, "Initial NRLink.");
  NRLinkCommand(NRLinkFlush);
  NRLinkCommand(NRLinkDefault);
  NRLinkCommand(NRLinkLongRange);
  NRLinkCommand(NRLinkSelectPF);
  TextOut(0, LCD_LINE5, "Commanding...");

  while(true)
  {
   NRLinkRunMacro(Motor_Ch1_A_FWD);
   PlayTone(440, 500);
   Wait(2000);
   NRLinkRunMacro(Motor_Ch1_A_Float);
   PlayTone(440, 500);
   Wait(2000);
   NRLinkRunMacro(Motor_Ch1_A_REV);
   PlayTone(440, 500);
   Wait(2000);
   NRLinkRunMacro(Motor_Ch1_A_Brake);
   PlayTone(440, 500);
   Wait(2000);
  }
}