/************************************************************************/
/*                                                                      */
/* Program Name: NRLink-lib.c                                           */
/* ===========================                                          */
/*                                                                      */
/* Copyright (c) 2008 by mindsensors.com                                */
/* Email: info (<at>) mindsensors (<dot>) com                           */
/*                                                                      */
/* This program is free software. You can redistribute it and/or modify */
/* it under the terms of the GNU General Public License as published by */
/* the Free Software Foundation; version 3 of the License.              */
/* Read the license at: http://www.gnu.org/licenses/gpl.txt             */
/*                                                                      */
/************************************************************************/

//definitions for NRLink

const ubyte NRLinkDataBytes       = 0x40;
const ubyte NRLinkSendCommandReg  = 0x41;
const ubyte NRLinkReadResult      = 0x42;
const ubyte NRLinkWriteData       = 0x42;


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 NRLinkMacro          = 0x52;

const ubyte Macro_Short_range    = 0x01;
const ubyte Macro_Long_Range     = 0x04;

const ubyte RCX_Macro_Power_off  = 0x07;
const ubyte RCX_Run_Program_1    = 0x09;
const ubyte RCX_Run_Program_2    = 0x0D;
const ubyte RCX_Run_Program_3    = 0x11;
const ubyte RCX_Run_Program_4    = 0x15;
const ubyte RCX_Run_Program_5    = 0x19;
const ubyte RCX_Stop_All_Program = 0x1D;

const ubyte RCX_Motor_A_FWD      = 0x21;
const ubyte RCX_Motor_A_REV      = 0x25;
const ubyte RCX_Motor_B_FWD      = 0x29;
const ubyte RCX_Motor_B_REV      = 0x2D;
const ubyte RCX_Motor_C_FWD      = 0x31;
const ubyte RCX_Motor_C_REV      = 0x35;
const ubyte RCX_Beep             = 0x39;

const ubyte PF_Motor_Ch1_A_Float    = 0x50;
const ubyte PF_Motor_Ch1_A_FWD      = 0x53;
const ubyte PF_Motor_Ch1_A_REV      = 0x56;
const ubyte PF_Motor_Ch1_A_Brake    = 0x59;

const ubyte PF_Motor_Ch1_B_Float    = 0x5C;
const ubyte PF_Motor_Ch1_B_FWD      = 0x5F;
const ubyte PF_Motor_Ch1_B_REV      = 0x62;
const ubyte PF_Motor_Ch1_B_Brake    = 0x65;

const ubyte PF_Motor_Ch2_A_Float    = 0x68;
const ubyte PF_Motor_Ch2_A_FWD      = 0x6B;
const ubyte PF_Motor_Ch2_A_REV      = 0x6E;
const ubyte PF_Motor_Ch2_A_Brake    = 0x71;

const ubyte PF_Motor_Ch2_B_Float    = 0x74;
const ubyte PF_Motor_Ch2_B_FWD      = 0x77;
const ubyte PF_Motor_Ch2_B_REV      = 0x7A;
const ubyte PF_Motor_Ch2_B_Brake    = 0x7D;

const ubyte PF_Motor_Ch3_A_Float    = 0x80;
const ubyte PF_Motor_Ch3_A_FWD      = 0x83;
const ubyte PF_Motor_Ch3_A_REV      = 0x86;
const ubyte PF_Motor_Ch3_A_Brake    = 0x89;

const ubyte PF_Motor_Ch3_B_Float    = 0x8C;
const ubyte PF_Motor_Ch3_B_FWD      = 0x8F;
const ubyte PF_Motor_Ch3_B_REV      = 0x92;
const ubyte PF_Motor_Ch3_B_Brake    = 0x95;

const ubyte PF_Motor_Ch4_A_Float    = 0x98;
const ubyte PF_Motor_Ch4_A_FWD      = 0x9B;
const ubyte PF_Motor_Ch4_A_REV      = 0x9E;
const ubyte PF_Motor_Ch4_A_Brake    = 0xA1;

const ubyte PF_Motor_Ch4_B_Float    = 0xA4;
const ubyte PF_Motor_Ch4_B_FWD      = 0xA7;
const ubyte PF_Motor_Ch4_B_REV      = 0xAA;
const ubyte PF_Motor_Ch4_B_Brake    = 0xAD;


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

void NRLinkRunMacro(tSensors port, byte id, byte  NRLinkMacroAdd)
{

  byte NRLinkMsg[5];

  // Build the I2C message
  NRLinkMsg[0] = 4;
  NRLinkMsg[1] = id;
  NRLinkMsg[2] = NRLinkSendCommandReg  ;
  NRLinkMsg[3] = NRLinkMacro;
  NRLinkMsg[4] = NRLinkMacroAdd;


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


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

void NRLinkSendCommand(tSensors port, byte id, byte  command)
{

	byte NRLinkMsg[5];

	// Build the I2C message
	NRLinkMsg[0] = 3;
	NRLinkMsg[1] = id;
	NRLinkMsg[2] = NRLinkSendCommandReg  ;
	NRLinkMsg[3] = command;

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


/////////////////////////////////////////////////////////////////////////////
//
//     Write a macro byte on the NRLink.
//
/////////////////////////////////////////////////////////////////////////////

void NRLinkWriteMacroByte(tSensors port, byte id, byte Location, byte data)
{

	byte NRLinkMsg[5];
	const byte MsgSize         = 0;
	const byte Address         = 1;
	const byte EEPROMLocation  = 2;
	const byte EEPROMData      = 3;


	// Build the I2C message
	NRLinkMsg[MsgSize]        = 3;
	NRLinkMsg[Address]        = id;
	NRLinkMsg[EEPROMLocation] = Location;
	NRLinkMsg[EEPROMData]     = data;

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

byte NRLinkPrintMacro(tSensors port, byte id, byte Location, byte lineNumber)
{

  byte i2cscanMsg[4];
	string s1, s2;
  byte replyMsg[12];

  // Build the I2C message
  i2cscanMsg[0] = 2;
  i2cscanMsg[1] = id;
  i2cscanMsg[2] = Location ;

  replyMsg[0]=0x00;

  while (nI2CStatus[port] == STAT_COMM_PENDING);   // Wait for I2C bus to be ready
  sendI2CMsg(port, i2cscanMsg[0], 6);                    // Send the message
  if(nI2CStatus[port] == ERR_COMM_BUS_ERR)return 0;
  while (nI2CStatus[port] == STAT_COMM_PENDING);   // Wait for I2C bus to be ready
  readI2CReply(port, replyMsg[0], 6);                // read all the six data bytes
  //if(replyMsg[0]==0x00) return 0;

  s1 = "m:";
	StringFormat (s2, "%s %x", s1, (int) (replyMsg[0]&0xFF));
	s1 = s2;
	StringFormat (s2, "%s %x", s1, (int) (replyMsg[1]&0xFF));
	s1 = s2;
	StringFormat (s2, "%s %x", s1, (int) (replyMsg[2]&0xFF));
	s1 = s2;
	StringFormat (s2, "%s %x", s1, (int) (replyMsg[3]&0xFF));
	s1 = s2;
	StringFormat (s2, "%s %x", s1, (int) (replyMsg[4]&0xFF));

	nxtDisplayTextLine(lineNumber,"%s", s2);

  return 1;
}