////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    Bluetooth Operational Tests
//
// RobotC provides access and control over the NXT Bluetooth link. This program contains many samples
// to illustrate this functionality.
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                  waitForBTCommandToComplete
//
// Most BT commands take several "time slices" to complete. The NXT CPU sends a command request to the
// Bluetooth chip. The chip processes the command and then sends a reply message back to the NXT CPU
// containing the results.
//
// The NXT firmware internally does a 'time out' test to check if the command has completed so a user
// application program can rely on this and not do its own testing. There are three different timeout
// values used by the firmware
//    0.5 second for commands that only operate on the internal variables in the bluetooth chip
//    2.0 seconds for commands that manipulate the bluetooth radio
//   30.0 seconds for commands like 'search', 'connect', etc that are setting up or tearing down links
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

TLoaderErrors nBTCmdErrorStatus;

TLoaderErrors waitForBTCommandToComplete()
{
	while (true)
	{
	  if (nBluetoothCmdStatus == INPROGRESS)
	    continue;

	  return nBluetoothCmdStatus;
	  wait1Msec(5); // Give other tasks a chance to run
	}
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                        Display Command Progress and Results
//
// Some common utility routines to display a command on the NXT LCD.
//
// And wait for the command to complete. Draw a progress bac on text line three.
//
// And stall for a bit after command completes so that you can read the results on the NXT display.
// Wait for a longer time when it is an error condition.
//
// Line 1 contains the command name
//
// Line 3 contains the command results
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

#define displayCommand(sCommandName) do {nxtDisplayTextLine(2, sCommandName);\
                                     nxtDisplayTextLine(4, "");} while (false)

#define displayResult(sResult)       nxtDisplayTextLine(4, sResult)


void displayBluetoothErrorStatus()
{
	const int kSuccessDelay =  500;
	const int kFailDelay    = 2000;

	switch (nBTCmdErrorStatus)
	{
	case SUCCESS:
		displayResult("Successful");
		wait1Msec(kSuccessDelay);
		PlaySound(soundBlip);
		break;

	case BTTIMEOUT:
		displayResult("Timeout");
		PlaySound(soundBeepBeep);
		wait1Msec(kFailDelay);
		break;

	case BTBUSY:
		displayResult("BT Busy");
		PlaySound(soundBeepBeep);
		wait1Msec(kFailDelay);
		break;

	case FILENOTFOUND:
		displayResult("NXT not found");
		PlaySound(soundBeepBeep);
		wait1Msec(kFailDelay);
		break;


	default:
	  nxtDisplayTextLine(3, "Error %04X", nBTCmdErrorStatus);
		PlaySound(soundBeepBeep);
		wait1Msec(kFailDelay);
	}
	return;
}

void waitForCommandCompletion()
{
	const int kProgressBarTop 		= 22;
	const int kProgressBarBottom 	= 17;

	int nIndex = 0;


	displayResult("Cmd underway");
	while (true)
	{
	  if (nBTCmdErrorStatus != INPROGRESS)
	    break;
	  nBTCmdErrorStatus = nBluetoothCmdStatus;

	  ++nIndex;
		if ((nIndex % 4) == 0)
		{
			nIndex %= 800;
			if (nIndex > 400)
			  nxtEraseRect(200 - nIndex / 4, kProgressBarTop, 99, 17);
			else
			  nxtFillRect(0, kProgressBarTop, nIndex / 4,17);
		}
	  wait1Msec(5);
	}

	//
	// Command is complete
	//
	nxtEraseRect(0, kProgressBarTop, 99, 17); // erase the progress rectangle
	displayBluetoothErrorStatus();

	return;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                        BluetoothOff()   and    BluetoothOn()
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

bool FactoryReset()
{
	displayCommand("Factory Reset");
	btFactoryReset(nBTCmdErrorStatus);
	waitForCommandCompletion();
	wait1Msec(3000);
	return (nBTCmdErrorStatus == SUCCESS);
}


bool BluetoothOff()
{
	displayCommand("Set BT Off");
	setBluetoothOff(nBTCmdErrorStatus);
	waitForCommandCompletion();
	return (nBTCmdErrorStatus == SUCCESS);
}


bool BluetoothOn()
{
	displayCommand("Set BT On");
	setBluetoothOn(nBTCmdErrorStatus);
	waitForCommandCompletion();
	return (nBTCmdErrorStatus == SUCCESS);
}

void DelayBetweenCommands()
{
	wait1Msec(1500);
	return;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                       Turn BlueTooth Visibility On and Off
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

bool BluetoothVisible(const bool bVisible)
{
	if (bVisible)
		displayCommand("Set Visible");
	else
	  displayCommand("Set InVisible");
	setBluetoothVisibility(nBTCmdErrorStatus, bVisible);
	waitForCommandCompletion();
	return (nBTCmdErrorStatus == SUCCESS);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                       Connect / Disconnect Bluetooth Devices
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

#define sName "NXT22"

void SetBrickName()
{
	displayCommand("Set Brick Name");
	setFriendlyName(nBTCmdErrorStatus, sName);
	waitForCommandCompletion();
	return;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                       Connect / Disconnect Bluetooth Devices
//
////////////////////////////////////////////////////////////////////////////////////////////////////////


const int nPort = 1;

void ConnectPort()
{
	displayCommand("Connect");
	btConnect(nBTCmdErrorStatus, nPort, sName);
	waitForCommandCompletion();
	return;
}

void DisconnectPort()
{
	displayCommand("Disconnect");
	btDisconnect(nBTCmdErrorStatus, nPort);
	waitForCommandCompletion();
	return;
}


////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                        Search Bluetooth Devices
//
////////////////////////////////////////////////////////////////////////////////////////////////////////


int nGetFoundDeviceCount()
{

}


void waitForSearchCompletion()
{
	const int kProgressBarTop 		= 22;
	const int kProgressBarBottom 	= 17;

	int nIndex = 0;


	displayResult("Search in Progress");
	while (true)
	{
	  if (nBTCmdErrorStatus != INPROGRESS)
	    break;
	  nBTCmdErrorStatus = nBluetoothCmdStatus;

	  ++nIndex;
		if ((nIndex % 4) == 0)
		{
			nIndex %= 800;
			if (nIndex > 400)
			  nxtEraseRect(200 - nIndex / 4, kProgressBarTop, 99, 17);
			else
			  nxtFillRect(0, kProgressBarTop, nIndex / 4,17);
		}
	  wait1Msec(5);
	}

	//
	// Command is complete
	//
	nxtEraseRect(0, kProgressBarTop, 99, 17); // erase the progress rectangle
	displayBluetoothErrorStatus();

	return;
}

void SearchBluetooth()
{
	displayCommand("Search");
	btSearch(nBTCmdErrorStatus);
	waitForSearchCompletion();
	return;
}

void StopSearchBluetooth()
{
	displayCommand("Stop Search");
	btStopSearch(nBTCmdErrorStatus);
	waitForCommandCompletion();
	return;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                        Main Task
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

task main()
{
	//
	// Test Ability to Turn Bluetooth On or Off
	//
	displayCommand(" Test Bluetooth ");
	bNxtLCDStatusDisplay = true;
	wait1Msec(2000);

	ConnectPort();
	DelayBetweenCommands();

	BluetoothOff();
	DelayBetweenCommands();
	FactoryReset();
	DelayBetweenCommands();
	SetBrickName();
	DelayBetweenCommands();
	BluetoothOn();
	DelayBetweenCommands();

	//
	// Test Ability to Make Bluetooth visible or invisible.
	//
	// NOTE: BT can be on, but invisible. "Invisible" means that it does not respond to
	//       "search for device" requests from other BT devices. But, if a device knows
	//       the address/name of this device then it can still make connections
	//
	BluetoothVisible(false);
	DelayBetweenCommands();
	BluetoothVisible(true);
	DelayBetweenCommands();

	//
	// Connect and Disconnect
	//
	ConnectPort();
	DelayBetweenCommands();
	DisconnectPort();
	DelayBetweenCommands();

	return;
}