//--------------------------------------------------------------------------------------------
/*
	Author: Colin Riley
	Date: 4-20-2015
	Project: Robotic Gopher Tortoise Scope
	Sponsor: Tall Timbers Research Station & Land Conservancy
	Description: 
			
			This program is designed to read inputs form a gamepad and send commands to 
			the rover. While it has the ability to read all gamepad commands, currently
			only the left joystick, directional pad, and a few buttons are used.

			This program was designed to be run on a Raspberry Pi running the Raspbian OS
			and read commands off of a Logitech Gamepad F310.
*/
//--------------------------------------------------------------------------------------------
//Include neccesary libraries

#include <SDL2/SDL.h>			//window creation and gamepad input reading features
//#include <SDL2/SDL_image.h>		//image loading and rendering features
#include <stdio.h>				//print text to console
#include <string>				//to hadle strings
#include <cmath>				//basic math function like inverse tangent
#include <iostream>				//print to console

#include <stdlib.h>             //sending system commands in Linux


#include "wiringPi.h"			//send commands over USB on raspberry pi
#include "wiringSerial.h"
//--------------------------------------------------------------------------------------------
//Function Declarations

bool init();						//Initialization routine run on start up
void close();						//Close routine run on shut down
char eventHandler(SDL_Event input, char cmd, bool &quit);	//handles events, ya' dummy
char motorControl(char cmd);	//convert joystick coordinates into motor commands
void sendcmd(char cmd);			//send commands over usb or prints commands to console
//--------------------------------------------------------------------------------------------
//Global variables

SDL_GameController* GameController = NULL;	//Game Controller handler

//These global strings can be helpful when dubugging, but are kept commented out for production.
//std::string command = "";	
//std::string prev_command = "";

//These variables are needed to set up serial communication over USB
//Look up wireing pi library for more detials
char device [] = "/dev/ttyACM0";	//Name or Arduino's usb port
int fd;
unsigned long baud = 9600;

//--------------------------------------------------------------------------------------------
//This function runs once at start up. It initializes the gamepad and serial communications
//with the Arduino. It returns success = true if everything initialzes correctly. It prints
//error messages and returns success = false if something fails to initialize. Success = false
//will cause the program to quit in main loop.
bool init()
{
	//Initialization flag
	bool success = true;

	//Initialize SDL
	if( SDL_Init( SDL_INIT_GAMECONTROLLER | SDL_INIT_TIMER ) < 0 )
	{
		printf( "SDL could not initialize! SDL Error: %s\n", SDL_GetError() );
		success = false;
	}
	else //if SDL initialized succesfully
	{
		//Load gamepad
		GameController = SDL_GameControllerOpen( 0 );

		if( GameController == NULL )
		{
			printf( "Warning: Unable to open game controller! SDL Error: %s\n", SDL_GetError() );
			success = false;
		}
	}

	std::cout<<"Initialized!\n";	//print initialization message to console

	fflush(stdout);	//clear serial data lines (just in case!)

	//open serial device (Arduino)
	if ((fd = serialOpen (device, baud)) < 0) 
	{
		printf ("Unable to open serail device\n");
		success = false;
	}
	//setup serial comms
	if (wiringPiSetup () == -1)
	{
		printf ("Unable to start wiringPi\n");
		success = false;
	}

	return success;
}
//End function "Init"
//--------------------------------------------------------------------------------------------
//This function runs once at shut down. It deallocates memory and shuts down the program 
// properly.
void close()
{
	//Close game controller
	SDL_GameControllerClose( GameController );
	GameController = NULL;

	//close serial comms
	serialClose(fd);

	//Quit SDL subsystems
	SDL_Quit();
}
//End function "close"
//--------------------------------------------------------------------------------------------
//This funciton determines the event type and then sends the proper command over usb to the
//Arduino Micro onboard the rover. 
//For more info, look up the SDL_Event's "caxis" and "cbutton".  

/*       Note:
  It is helpful to uncomment the "command = ..." lines when debugging. These commands can
  then be printed in the sendcmd function to verify code.
  
  "cmd = ..." lines should not be uncommented unless the Arduino code is updated to include
  these added commands in it decision logic.
*/

char eventHandler(SDL_Event input, char cmd, bool &quit)
{
	//If user requests quit
	if( input.type == SDL_QUIT )
	{
		quit = true;
	}
	//If input is of Controller axis motion type
	//This includes the two joysticks and the two triggers
	else if( input.type == SDL_CONTROLLERAXISMOTION )
	{				
		
		if (input.caxis.axis == SDL_CONTROLLER_AXIS_TRIGGERLEFT)
		{
			//command = "Left Trigger";
			//cmd = 83;
		}
		else if (input.caxis.axis == SDL_CONTROLLER_AXIS_TRIGGERRIGHT)
		{
			//command = "Right Trigger";
			//cmd = 84;
		}
		else
		{
			cmd = motorControl(cmd); //this function will deal with joystick motion and return cmd value
		}
	}
	//If a controller button has been pushed down
	else if( input.type == SDL_CONTROLLERBUTTONDOWN )
	{
		switch(input.cbutton.button){

		case SDL_CONTROLLER_BUTTON_A:
			//play live video-feed on UI screen using mplayer
			command = "Button A";
			//cmd = 93;
			system("mplayer tv:// -tv driver=v4l2:norm=PAL-M:device=/dev/video0:fps=1:noaudio:outfmt=uyvy:fps=10 -vo sdl -fs -vf format=y8,scale -hardframedrop &");
			//system("./vid_feed.exe");
			break;
		case SDL_CONTROLLER_BUTTON_B:
			//close video-feed
			command = "Button B";
			//cmd = 94;
			system("pkill mplayer");
			break;
		case SDL_CONTROLLER_BUTTON_X:
			command = "Button X";
			//cmd = 95;
			//system("mplayer tv:// -tv driver=v4l2:norm=PAL-M:device=/dev/video0:fps=1:noaudio:outfmt=uyvy:fps=10 -vo jpeg -frames 3 -fs -vf format=y8,scale -hardframedrop &");
			break;
		case SDL_CONTROLLER_BUTTON_Y:
			//request temp and humidity data from Arduino
			command = "Button Y";
			cmd = 96;
			break;
		case SDL_CONTROLLER_BUTTON_BACK:
			//shutdown raspberry pi
			command = "Back";
			//cmd = 88;
			system("sudo shutdown -h now");
			break;
		case SDL_CONTROLLER_BUTTON_GUIDE:
			//quit program
			//command = "Guide";
			quit = true;
			//system("sudo shutdown -h now");
			break;
		case SDL_CONTROLLER_BUTTON_START:
			//command = "Start";
			//cmd = 89;
			break;
		case SDL_CONTROLLER_BUTTON_LEFTSTICK:
			//command = "Left Stick";
			//cmd = 90;
			break;
		case SDL_CONTROLLER_BUTTON_RIGHTSTICK:
			//command = "Right Stick";
			//cmd = 91;
			break;
		case SDL_CONTROLLER_BUTTON_LEFTSHOULDER:
			//command = "Left Shoulder";
			//cmd = 85;
			break;
		case SDL_CONTROLLER_BUTTON_RIGHTSHOULDER:
			//command = "Right Shoulder";
			//cmd = 86;
			break;
		case SDL_CONTROLLER_BUTTON_DPAD_UP:
			//center camera
			command = "Dpad Up";
			cmd = 99;
			break;
		case SDL_CONTROLLER_BUTTON_DPAD_DOWN:
			//command = "Dpad Down";
			//cmd = 87;
			break;
		case SDL_CONTROLLER_BUTTON_DPAD_LEFT:
			//pan camera left
			command = "Dpad Left";
			cmd = 98;
			break;
		case SDL_CONTROLLER_BUTTON_DPAD_RIGHT:
			//pan camera right
			command = "Dpad Right";
			cmd = 97;
			break;
		}
	}

	return (cmd);
}
//End of function "eventHandler"
//--------------------------------------------------------------------------------------------
//This function converts joystick coordinates into motor commands and sends them over usb.
char motorControl(char cmd)
{
	const double min = 2500;	//deadzone
	const double mid = 20700;	//line between fast a slow speed

	//read in x and y coordinates. Flip y coordinate to make more sense
	double yCor = -1*SDL_GameControllerGetAxis(GameController, SDL_CONTROLLER_AXIS_LEFTY);
	double xCor = SDL_GameControllerGetAxis(GameController, SDL_CONTROLLER_AXIS_LEFTX);
	
	//intermediate calculations to determine joystick angle
	double opp = yCor/xCor;
	double theta = theta = (atan( opp )) * (180.0/3.14159);;
	double joystickAngle = 0;

	//determine joystick angle based on quadrant of joystick.
	//atan returns result between -90 and 90 degrees.
	//this logic converts joystick angle to be between 0 and 360 degrees.
	if (xCor == 0 && yCor == 0)
	{
		joystickAngle = 0;
	}
	//first quadrant
	else if (xCor >= 0 && yCor >= 0 )
	{
		joystickAngle = theta;
	}
	//second
	else if (xCor < 0 && yCor >= 0 )
	{
		joystickAngle = 180 + theta;
	}
	//third
	else if (xCor < 0 && yCor < 0 )
	{
		joystickAngle = 180 + theta;
	}
	//fourth
	else if (xCor >= 0 && yCor < 0 )
	{
		joystickAngle = 360 + theta;
	}

	//calculate joystick magnitude
	double joystickMag = sqrt( (xCor * xCor) + (yCor * yCor));

	//For debugging
	//printf ("Angle: %f\n", joystickAngle);
	//printf ("Mag: %f\n", joystickMag);

	//Finally, joystick angle and magnitude is used to determine the direction
	//and speed the rover should move in.
	if ( joystickMag < min )
	{
		command = "Break";
		cmd = 100;
	}
	else if ( (joystickAngle <= 10) || (joystickAngle >= 350))
	{
		if ( joystickMag < mid )
		{
			command = "Slow Right";
			cmd = 101;
		}
		else
		{
			command = "Fast Right";
			cmd = 110;
		}	
	}
	else if ( (joystickAngle >= 11) && (joystickAngle <= 79))
	{
		if ( joystickMag < mid )
		{
			command = "Slow Forward Right";
			cmd = 102;
		}
		else
		{
			command = "Fast Forward Right";
			cmd = 111;
		}	
	}
	else if ( (joystickAngle >= 80) && (joystickAngle <= 100))
	{
		if ( joystickMag < mid )
		{
			command = "Slow Forward";
			cmd = 103;
		}
		else
		{
			command = "Fast Forward";
			cmd = 112;
		}	
	}
	else if ( (joystickAngle >= 101) && (joystickAngle <= 169))
	{
		if ( joystickMag < mid )
		{
			command = "Slow Forward Left";
			cmd = 104;
		}
		else
		{
			command = "Fast Forward Left";
			cmd = 113;
		}	
	}
	else if ( (joystickAngle >= 170) && (joystickAngle <= 190))
	{
		if ( joystickMag < mid )
		{
			command = "Slow Left";
			cmd = 105;
		}
		else
		{
			command = "Fast Left";
			cmd = 114;
		}	
	}
	else if ( (joystickAngle >= 191) && (joystickAngle <= 259))
	{
		if ( joystickMag < mid )
		{
			command = "Slow Backward Left";
			cmd = 106;
		}
		else
		{
			command = "Fast Backward Left";
			cmd = 115;
		}	
	}
	else if ( (joystickAngle >= 260) && (joystickAngle <= 290))
	{
		if ( joystickMag < mid )
		{
			command = "Slow Backward";
			cmd = 107;
		}
		else
		{
			command = "Fast Backward";
			cmd = 116;
		}	
	}
	else if ( (joystickAngle >= 291) && (joystickAngle <= 349))
	{
		if ( joystickMag < mid )
		{
			command = "Slow Backward Right";
			cmd = 108;
		}
		else
		{
			command = "Fast Backward Right";
			cmd = 117;
		}	
	}

	return (cmd);
}
//End of funciton "motorControl"
//--------------------------------------------------------------------------------------------
//This function sends characters over usb using the wireing pi libraries. It also reads in
//temp and humidty data.
void sendcmd(char cmd)
{
	if (cmd == 96)	//request temp and humidty data from rover
	{
		char humidity [10];
		char celsius [10];
		char fahrenheit [10];
		char receivedChar = 0;
		int i = 0;
		int j = 0;
		int k = 0;
		
		serialFlush(fd);	//clear serail lines
		
		serialPutchar (fd, cmd);	//send command
	
		while ( serialDataAvail(fd) < 1 )	//delay until arduino sends data
		{
			SDL_Delay(1);
		}
		
		receivedChar = serialGetchar(fd); //read in first chracter
		
		do
		{
			humidity[i] = receivedChar;			//store character as humidity data
			i++;
			receivedChar = serialGetchar(fd);
		} while ( receivedChar != ',');			//store characters until , is reached
		
		receivedChar = serialGetchar(fd);		//begin reading temperature data
		
		do
		{
			celsius[j] = receivedChar;
			j++;
			receivedChar = serialGetchar(fd);
		} while ( receivedChar != ',');
		
		receivedChar = serialGetchar(fd);
		
		do
		{
			fahrenheit[k] = receivedChar;
			k++;
			receivedChar = serialGetchar(fd);
		} while ( receivedChar != ',');
		
		//print data to the console
		std::cout<<"Humidity: ";
		
		for (int lcv = 0; lcv < i; lcv++)
		{
			std::cout<<humidity[lcv];
		}
		
		std::cout<<"\n"<<"Celsius: ";
		
		for (int lcv = 0; lcv < j; lcv++)
		{
			std::cout<<celsius[lcv];
		}
		
		std::cout<<"\n"<<"Fahrenheit: ";
		
		for (int lcv = 0; lcv < k; lcv++)
		{
			std::cout<<fahrenheit[lcv];
		}
		
		std::cout<<"\n";
	} 
	else	//send command to rover and move on, if we are not expecting data in reply
	{
		serialPutchar (fd, cmd);
	}

	//Useful for debugging
	//std::cout<<command<<"\n";
	//printf("%c\n", cmd);
}
//End of function "sendcmd"
//--------------------------------------------------------------------------------------------
//main control logic loop
int main( int argc, char* args[] )
{
	//Run initialization function and check for success
	if( !init() )
	{
		printf( "Failed to initialize!\n" );
	}
	
	else	//if initialization is successful
	{		
		//displaying camera feed
		//system("mplayer tv:// -tv driver=v4l2:norm=PAL-M:device=/dev/video0 -vo sdl -framedrop");
	
		bool quit = false;	//flag to exit main loop
		SDL_Event input;	//store input commands
		char cmd = 0;		//commands to be sent to arduino
		char prev_cmd = 0;	//store old commands
		int WatchDog = SDL_GetTicks() + 300;	//watchdog timer

		//While user hasn't quit
		while( !quit )
		{
			while( SDL_PollEvent( &input ) != 0 )	//poll gamepad, store events in input variable
			{
				cmd = eventHandler( input, cmd, quit);	//determine event type and corresponding cmd to send

				if (cmd != 0)
				{
					//limit number of commands we send for efficiency and power savings
					if ( (cmd != prev_cmd) || (SDL_TICKS_PASSED(SDL_GetTicks(), WatchDog)))	
					{
						sendcmd(cmd);
						WatchDog = SDL_GetTicks() + 300; //reset watchdog timer
						//prev_command = command;
						prev_cmd = cmd;
					}
				}
				
				//command = "";
				cmd = 0;

			}//end while input event present
		}//end while not quite
	}

	//For Debugging
	//SDL_Delay(4000);
	
	//Free resources and close SDL
	close();
	//system("sudo shutdown -h now");
	return 0;
}
//End "main"
//--------------------------------------------------------------------------------------------