/*
Alvaro Pintado
pintado@unlv.nevada.edu

Objective: This program opens up a COM port for Bluetooth communication with a LEGO NXT Brick.
The program sends direct commands in the form of byte arrays to be interpreted through the
LEGO Mindstorms NXT Communication Protocol.

Notes:
-When writing commands to the LEGO NXT, it is advised that you do not request a response
in order to avoid the time delay of the Bluetooth chipping switching modes from receiving
to transmitting (30 - 60 ms). Unfortunately, I was unable to get that command byte working
as the hexadecimal encoding used in the Serial Port class only encodes integers up to 127.
For my application, the latency was not a significant issue (and won't be in most
applications). If the user would like to experiment with sending commands that are over
127, they should look into changing the encoding for the Serial Port class to a different
encoding scheme such as UTF-8.

*/

// Necessary header files and namespaces in order to use the Serial Port Class

#using <System.dll>
#include <Windows.h>
#include <string>

using namespace System;
using namespace System::IO::Ports;
using namespace System::Threading;
using namespace std;

int main() {

	//Settings for COM Port
	Int32 baudRate = 9600;
	Int32 readTimeout = 500; // time limit for attempting to write to port before exiting
	Int32 writeTimeout = 500; // time limit for attempting to read from port before exiting

	//Requests Port Number from user
	Console::Write("Enter NXT Port and press Enter: ");
	String^ portNum = Console::ReadLine();
	Console::WriteLine("Opening COM port ...");
	Console::WriteLine();

	//Creates Serial Port object named "NXT" for communication over Bluetooth
	SerialPort^ NXT = gcnew SerialPort(portNum, baudRate, Parity::None, 8, StopBits::One); //creates Serial Port object named "NXT"
	NXT->ReadTimeout = readTimeout; // sets read timeout
	NXT->WriteTimeout = writeTimeout; //sets write timeout

									  // Opens port for communication with NXT Brick over Bluetooth
	NXT->Open();
	Console::WriteLine(portNum + " opened for Bluetooth communication.");
	Console::WriteLine();
	Sleep(3000);

	// Byte array for a direct command to the NXT Brick to play a tone
	// Byte 00: Least Significant Byte:				12th byte
	// Byte 01: Most Significant Bit:				0th Byte
	// Byte 02: Command response requested:			        0x80 for response, 0x00 for no response
	// Byte 03: Mode byte:				                0x04 for controlling a motor
	// Byte 04: Power set in percentages:	                        0x64 is 100 (hex)
	// Byte 05: Output mode byte:					0x07 for all ON
	// Byte 06: Regulation mode byte:				0x00 for idle
	// Byte 07: Turn ratio:						0x00 
	// Byte 08: Motor run state:					0x20 for running
	// Byte 09 - 13: Tacho limit:					0x00 0x00 0x00 0x00 for running forever
	// See LEGO Mindstorms NXT Communication Protocol documentation for details
	// motorA = 0, motor b = 1, motor c = 1
	wchar_t motorL = [left motor port]; 
	wchar_t motorR = [right motor port];
	cli::array<wchar_t, 1>^ driveL = { 0x0C, 0x00, 0x00, 0x04, motorL, 0x64, 0x07, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00 };
	cli::array<wchar_t, 1>^ driveR = { 0x0C, 0x00, 0x00, 0x04, motorR, 0x64, 0x07, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00 };
	cli::array<wchar_t, 1>^ stopL = { 0x0C, 0x00, 0x00, 0x04, motorL, 0x00, 0x07, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00 };
	cli::array<wchar_t, 1>^ stopR = { 0x0C, 0x00, 0x00, 0x04, motorR, 0x00, 0x07, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00 };


	//Writes direct command to the NXT Brick to drive two motors forward
	Console::WriteLine("Drive forward for 1 second");
	Sleep(5000);
	NXT->Write(driveL, 0, 14);
	NXT->Write(driveR, 0, 14);
	Sleep(1000);

	//Writes direct command to the NXT Brick to stop two motors
	Console::WriteLine("Stop motors");
	Sleep(1000);
	NXT->Write(stopL, 0, 14);
	NXT->Write(stopR, 0, 14);

	//Closes the port and exits the program
	Console::WriteLine("Closing program...");
	Sleep(5000);
	NXT->Close();
	exit(0);
}