//===================================================================
// File:           Joystick.cpp
// Name:           Robert Fitzgerald
// Date:           3/8/2006
// Description:    Joy Stick program©
//===================================================================
#include <iostream.h>
#include <windows.h>

//=====================================
// Global Constants
//=====================================

const char * APP_NAME			= "R/C Control Program";
const char * APP_VERSION		= "[Build 0.01]";
const char * APP_AUTHOR			= "Robert Fitzgerald";
const char * APP_COPYRIGHT1		= "Copyright (c) 2006";
const char * APP_COPYRIGHT2		= "Robert Fitzgerald, CMPLUS Technologies, Inc.";
const char * APP_COPYRIGHT3		= "All Rights Reserved.";

const char * PORT_NAME			= "COM1:";

const int    NUM_CHANNELS		= 8;

const int CHANNEL_STEERING		= 0;
const int CHANNEL_THROTTLE		= 1;

//=====================================
// Function: main()
//=====================================
int main (int argc, char * argv [])
{
	JOYINFOEX		jixStick;
	HANDLE			hPort;
	DCB				myDCB;
	COMMTIMEOUTS	myTimeOuts;

	unsigned char rcMsg[NUM_CHANNELS];
	unsigned long bytesWritten;
	double feed = 0;
	int speed = 504;
    int xpos;
	int ypos;
	int zpos;
	int rpos;
	int upos;
	int vpos;
	bool running;

   /* ------------------------------------ *
	*  Show program banner                 *
	* ------------------------------------ */

	cout << endl
		 << APP_NAME		<< " "
		 << APP_VERSION		<< endl
		 << APP_COPYRIGHT1	<< endl
		 << APP_COPYRIGHT2	<< endl
		 << APP_COPYRIGHT3	<< endl
		 << endl;
	
   /* ------------------------------------ *
	*  Open connection to port             *
	* ------------------------------------ */

	cout << "Opening port'" << PORT_NAME << "' ... ";
	hPort = CreateFile(PORT_NAME, GENERIC_READ | GENERIC_WRITE, 0, 0,
					  OPEN_EXISTING, FILE_FLAG_WRITE_THROUGH, 0);

	if (hPort == INVALID_HANDLE_VALUE){
		cout << endl << "Unable to create connection." << endl;
		return (0);
	}

	cout << "opened" << endl;

   /* ------------------------------------ *
	*  Set Port state values               *
	* ------------------------------------ */	
	cout << "Setting DCB values ... ";
	if (!GetCommState(hPort, &myDCB)) {
		cout << endl << "Unable to set port values." << endl;
	}

	myDCB.DCBlength			= sizeof(myDCB);
	myDCB.BaudRate			= CBR_19200;
	myDCB.fBinary			= TRUE;
	myDCB.fParity			= NOPARITY;
	myDCB.fDtrControl		= DTR_CONTROL_ENABLE;
	myDCB.fRtsControl		= RTS_CONTROL_ENABLE;
	myDCB.ByteSize			= 8;
	myDCB.Parity			= NOPARITY;
	myDCB.StopBits			= ONESTOPBIT;
	myDCB.fOutxCtsFlow		= TRUE;
	myDCB.fOutxDsrFlow		= FALSE;
	myDCB.fOutX				= FALSE;
	myDCB.fInX				= FALSE;
	myDCB.fDsrSensitivity	= FALSE;
	myDCB.fTXContinueOnXoff	= TRUE;
	myDCB.fErrorChar		= FALSE;
	myDCB.fNull				= FALSE;
	myDCB.fAbortOnError		= FALSE;

	if (!SetCommState(hPort, &myDCB)) {
		cout << endl << "Unable to set poert values." << endl;
		return (0);
	}
	cout << "set" << endl;

   /* ------------------------------------ *
	*  Set Port state values               *
	* ------------------------------------ */

	cout << "Setting timeout values ... ";
	if (!GetCommTimeouts(hPort, &myTimeOuts)) {
		cout << endl << "Unable to get communication timeouts." << endl;
		return (0);
	}

	myTimeOuts.ReadIntervalTimeout			= 100;
	myTimeOuts.ReadTotalTimeoutConstant		= 100;
	myTimeOuts.ReadTotalTimeoutMultiplier	= 0;
	myTimeOuts.WriteTotalTimeoutConstant	= 0;
	myTimeOuts.WriteTotalTimeoutMultiplier	= 0;

	if (!SetCommTimeouts(hPort, &myTimeOuts)) {
		cout << endl << "Unable to set communication myTimeOuts." << endl;
		return (0);
	}
	cout << "set" << endl;

   /* ------------------------------------ *
	*  Clear & Set Request to send (RTS)   *
	* ------------------------------------ */

	EscapeCommFunction(hPort, CLRBREAK);
	Sleep(60);
	EscapeCommFunction(hPort, CLRRTS);
	Sleep(60);
	EscapeCommFunction(hPort, SETRTS);
	Sleep(60);

   /* ------------------------------------ *
	*  Initialize all channel              *
	* ------------------------------------ */

	cout << "Initializing all channels ... ";
	for (int i = 0; i < NUM_CHANNELS; i++)
		rcMsg[i] = (unsigned char) 125;

//	if (!WriteFile(hPort, rcMsg, sizeof(rcMsg), &bytesWritten, 0)) {
//		cout << endl << "Unable to initialize channels - write failed." << endl;
//		return (0);
//	}

	cout << "done" << endl;

   /* ------------------------------------ *
	*  Run R/C Session                     *
	* ------------------------------------ */

	cout << endl << "PC -> R/C session ... running ";
	cout << endl;

	running = true;
	for (; running == true; )
	{
		memset(&jixStick, 0x00, sizeof(jixStick));
		jixStick.dwSize  = sizeof(JOYINFOEX);
		jixStick.dwFlags = JOY_RETURNALL;
		joyGetPosEx(JOYSTICKID1, &jixStick);

//		rcMsg[CHANNEL_STEERING] = (unsigned char) ((jixStick.dwXpos/262));
//		rcMsg[CHANNEL_THROTTLE] = (unsigned char) (fabs(feed-(jixStick.dwZpos/speed)));
//		WriteFile(hPort, rcMsg, sizeof(rcMsg), &bytesWritten, 0);
//		FlushFileBuffers(hPort);

//		100 - 250 stop forward
//		100 - 0   stop reverse

		xpos = (jixStick.dwXpos/262);
		ypos = (jixStick.dwYpos/262);
		zpos = (fabs(feed-(jixStick.dwZpos/speed)));

		rpos = (jixStick.dwRpos/262);
		upos = (jixStick.dwUpos/262);
		vpos = (jixStick.dwVpos/262);

	    cout << "X-Y-Z Values: "
			<< xpos << ", " 
			<< ypos << ", " 
			<< zpos << ", "
			<< rpos << ", " 
			<< upos << ", " 
			<< vpos << endl;

		if ((jixStick.dwButtons & JOY_BUTTON3)) {
			speed = 504;
			feed = 0;
//			650 346 521 435 0.0019074 504 542
//			PlaySound("./battle032.wav", 0, SND_SYNC);
		}

		if ((jixStick.dwButtons & JOY_BUTTON4)) {
			speed = 542;
			feed = 250;
//			PlaySound("./shotgun2.wav", 0, SND_SYNC);
		}

		if ((jixStick.dwButtons & JOY_BUTTON1))
			running = false;
//		varibles loops arrays functions 

//		if ((jixStick.dwButtons & JOY_BUTTON4)) {
//			cout << "Button 4 pressed" << endl;
//			PlaySound("./shotgun.wav", 0, SND_SYNC);
//		}

	}

	cout << endl << "PC -> R/C session ... stopped ";
	cout << endl;
	cout << endl;

   /* ------------------------------------ *
	*  Reset all channels                  *
	* ------------------------------------ */
	
	cout << "Reseting all channels ... ";
	for (int i = 0; i < NUM_CHANNELS; i++)
		rcMsg[i] = (unsigned char) 125;

//	if (!WriteFile(hPort, rcMsg, sizeof(rcMsg), &bytesWritten, 0)) {
//		cout << endl << "Unable to initialize channels - write failed." << endl;
//		return (0);
//	}

	cout << "done" << endl;

   /* ------------------------------------ *
	*  Close connection to port and exit   *
	* ------------------------------------ */
	cout << "Closing port '" << PORT_NAME << "' ... ";
	if (CloseHandle(hPort))
		cout << "closed" << endl;
	else
		cout << "Unable to close connection." << endl;

	cout << "Exiting program. " << endl;
	
	return 0;
}