#ifdef HAVE_CONFIG_H
#  include <config.h>
#  include <math.h>
#  include <pthread.h>
#  include <stdlib.h>
#define _REENTRANT
#endif
#include <string.h>
#include <gnome.h>
#include "EventHandlers.h"
#include "GUI.h"
#include "Support.h"
#include "RobotAPI.h"

RobotCommand *IncomingCommand;
int TranslatedOrientation;
int TurnRate = 0;

int TesterClass::runMe(void)
{
	int Error;
	printf("WAITING FOR NETWORK CONNECTION\n");

	ConfigSys Configuration;
	Configuration.loadConfiguration();
	
	APIC.setReceiver(this);
	APIC.setPort(Configuration.PortNumber);
	gdk_threads_enter();
	Error = APIC.establishConnection();
	if(Error == -1)
	{
		printf("ERROR ESTABLISHING CONNECTION\n");
		exit(-1);
	}
	else
	{
		g_print("CONNECTION ESTABLISHED\n");
	}
	gdk_threads_leave();
	return(0);
}

void TesterClass::changeOrientation(int orient)
{
	strcpy(Command.CommandName,"CHANGE_ORIENTATION");
	if(orient > 90)
	{
		orient = 450 - orient;
		TranslatedOrientation = orient;
	}
	else
	{
		orient = 90 - orient;
		TranslatedOrientation = orient;
	}
	Command.Value = orient;	
	ErrorFlag = APIC.sendCommand(Command);
	printf("CHANGE_ORIENTATION to %d sent with Errorflag = %d\n",orient,ErrorFlag);
}

void TesterClass::changeSpeed(int RobotSpeed)
{
	strcpy(Command.CommandName,"CHANGE_SPEED");
	Command.Value= RobotSpeed;
	ErrorFlag = APIC.sendCommand(Command);
};

void TesterClass::statusCheck()
{
	strcpy(Command.CommandName, "STATUS_CHECK");
	ErrorFlag = APIC.sendCommand(Command);
}


void TesterClass::steerMotor(int Val)
{
	strcpy(Command.CommandName, "STEER_MOTOR");
	Command.Value = Val;
	ErrorFlag = APIC.sendCommand(Command);
}

void TesterClass::receiveCommand(char InstructionCode, RobotCommand* NewCommand)
   {
      static int first = 0;
   printf("\nTC-Incoming command = %c\n", InstructionCode);   
   
   switch(InstructionCode)
      {
      case 'F':
         // CURRENT_STATUS
	 if(first != 0)
	 {
	 IncomingCommand = new RobotCommand;
	 }
	 first = 1;
	 IncomingCommand->copyCommand(*NewCommand);
         g_print("Current status...\n");
         g_print("Current Speed = %d\n",NewCommand ->ValueB);
         g_print("Current orientation = %d\n",NewCommand ->ValueC);
	 delete NewCommand;
         break;
         
      default:
         printf("Invalid Instruction Code\n");
	 delete NewCommand;
         return;      
      }
   }






static GtkWidget *DrawingSurface;
static int scrollrate = 0;
static gint orientation = 0;
static float PointX = 300;
static float PointY = 220;
TesterClass Net;
int StartClicked;

void draw_Grid(GtkWidget *area, gfloat xspeed, gfloat yspeed)
{
        static GdkGC *Black_GC, *Yellow_GC,*Red_GC;
	int x,y,i=0, p=0;
	int loopcounter =0;
	GdkPixmap *ScreenBuffer;
	gchar Speed[50];
	gchar Scale[15];
	GdkColor Color,Red;
	int sizex = 0, sizey = 0;
	static int size = 1;

	sprintf(Scale,"Scale: %dx",size);
	sprintf(Speed,"Orientation Rate: %d / Desired Speed: %d",TurnRate,scrollrate);	
	ScreenBuffer = gdk_pixmap_new(GTK_WIDGET(area) -> window, 800, 600, -1);

	
	
/////////////////////////////////////////////////////////////////////////
//Setup Colors
////////////////////////////////////////////////////////////////////////

	gdk_color_parse("Yellow",&Color);
	gdk_colormap_alloc_color(gdk_colormap_get_system(), &Color, FALSE, TRUE);

	gdk_color_parse("Red",&Red);
	gdk_colormap_alloc_color(gdk_colormap_get_system(), &Red, FALSE, TRUE);

////////////////////////////////////////////////////////////////////////
	//Setup Black graphic context
///////////////////////////////////////////////////////////////////////

	Black_GC = GTK_WIDGET(area)->style->black_gc;
	
	
	gdk_draw_rectangle(ScreenBuffer, Black_GC, TRUE, 0,0,800,600);

////////////////////////////////////////////////////////////////////////
	//Initialize Yellow Graphics Context
///////////////////////////////////////////////////////////////////////

	Yellow_GC = gdk_gc_new(area->window);
	gdk_gc_set_foreground(Yellow_GC, &Color); 
	
	Red_GC = gdk_gc_new(area -> window);
	gdk_gc_set_foreground(Red_GC, &Red);	
	
//////////////////////////////////////////////////////////////////////
	//Draw Checker Board
//////////////////////////////////////////////////////////////////////
		for(y=0; y < 700; y+=40)
		  for(x=0; x < 800; x+=80)
		   {
		      if((y % 80) == 0)
		      {
			 gdk_draw_rectangle(ScreenBuffer, Yellow_GC, TRUE, x, y, 40, 40);
		      }
		      else
		      {
			 gdk_draw_rectangle(ScreenBuffer, Yellow_GC, TRUE, x +40 ,y, 40,40);
		      }
		   }
		
///////////////////////////////////////////////////////////////////////////
		//Calculate Movement
//////////////////////////////////////////////////////////////////////////
		
 		if((xspeed != 0 || yspeed != 0) && StartClicked)
		{
		        Net.changeOrientation(orientation);	  
		        Net.changeSpeed(scrollrate);	
			Net.statusCheck();
			while(IncomingCommand -> ValueC != TranslatedOrientation)
			{ 
			   g_print("Turning Robot\n");
			}
			delete IncomingCommand;
			while(PointX != 300 && PointY != 220)
			{
		gdk_draw_string(area->window, area -> style -> font, Red_GC, 360,420, Speed);
			   draw_Arrow(area, orientation);
			   if(loopcounter == abs(scrollrate))
			   {
			      modify_Path(xspeed, yspeed);
			      loopcounter = 0;
			   }
			   loopcounter++;
			   plot_Path(area);
			   gdk_draw_pixmap(GTK_WIDGET(area) -> window, Yellow_GC, ScreenBuffer,80+ (i), 80- p,0,0,640,440);
			   draw_Arrow(area,orientation);
			   if(abs((int)rint(xspeed)) > 0 && abs((int)rint(xspeed)) < 3)
			   {
				   sizex = 1;
				   i += 1;
			   }
			   else
			   {
				sizey = 1;
			   	i += (int)rint(((double)xspeed/5.0));
			   }
			   if(abs((int)rint(yspeed)) > 0 && abs((int)rint(yspeed)) < 3)
			   {
				   p += 1;
			   }
			   else
			   {
			   	p += (int)rint(((double)yspeed/5.0));
			   }
			   if(i >  80 || i < -80)
			   {
				i = 0;
	      	           } 
			   if( p > 80 || p < -80)
			   {
				p = 0;
			   }
 			   if((xspeed) > (yspeed))
			   {
				size = (int)(xspeed);
			   }
			   else
			   {	
				size = (int)(yspeed);
			   }				
			sprintf(Scale, "Scale: %dx",size);
		gdk_draw_string(area->window, area -> style -> font, Red_GC, 20,420, Scale);
		   	}
		sleep(1);
		}
		else
		{
			gdk_draw_pixmap(GTK_WIDGET(area) -> window, Yellow_GC, ScreenBuffer,40,40,0,0,640,480);
  		}
		if(StartClicked == 1)
		{
		   Net.changeSpeed(0);
		}
		gdk_draw_string(area->window, area -> style -> font, Red_GC, 360,420, Speed);

		gdk_draw_string(area->window, area -> style -> font, Red_GC, 20,420, Scale);
		draw_Arrow(area, orientation);
		plot_Path(area);
		StartClicked = 0;
}


void draw_Arrow(GtkWidget *area, gint orientation)
{
	
	gint xpoint = 0, ypoint = 0, xpoint2 = 0, ypoint2 = 0;
	float radians=0;
	GdkGC *Green_GC;
	GdkColor Green;

	gdk_color_parse("Green",&Green);
	gdk_colormap_alloc_color(gdk_colormap_get_system(),&Green,FALSE,TRUE);

	Green_GC = gdk_gc_new(area -> window);
	gdk_gc_set_foreground(Green_GC, &Green);

	if(orientation == 0)
	{
		radians = 0;
	}
	else
	{
	   radians = (orientation * 22.0)/(180.0 * 7.0);
	}
	xpoint = (int)(20 * sin(radians));
	ypoint = (int)(20 * cos(radians));
	

	ypoint2 = ypoint;
	ypoint = 0 - ypoint;
	xpoint2 = 0 - xpoint;
	
	
	gdk_draw_line(area -> window, Green_GC,300,220,xpoint+300,ypoint+220 );
	gdk_draw_line(area -> window, Green_GC,300,220,xpoint2 + 300, ypoint2+220); 
	gdk_draw_arc(area -> window, Green_GC, FALSE, 280 , 200, 40, 40, 0, 360 * 64); 
	gdk_draw_arc(area -> window, Green_GC,TRUE,300 + xpoint -5 , 220 + ypoint -5 , 10, 10, 0 , 360 * 64);
}
	
void  scroll_map()
{
	float xspeed = 0;
	float yspeed = 0;
	float radians = 0;
	
	
	calc_Path(scrollrate);
	radians = (orientation * 22.0)/(180.0 * 7.0);
	
	xspeed = ((double)scrollrate * sin(radians));
	yspeed = ((double)scrollrate * cos(radians));
	draw_Grid(DrawingSurface, xspeed, yspeed);
}	

void calc_Path(int rate)
{
	int pathstart = 1;
	double XCoor, YCoor;
	double Ratio;
	XCoor = (double)300.0 - PointX;
	YCoor = (double)220.0 - PointY;
	Ratio = fabs(XCoor/YCoor); 
	++pathstart;
	orientation = (int)((atan(Ratio) * 180.0 * 7.0) / 22.0);
	if(XCoor > 0.0 && YCoor > 0.0)
	{
		orientation = 360 - orientation;
	}
	if(XCoor > 0.0 && YCoor < 0.0)
	{
		orientation += 180;
	}
	if(XCoor < 0.0 && YCoor < 0.0)
	{
		orientation = 180 - orientation;
	}
	if(orientation > 360 || orientation < 0)
	{
		orientation = 0;
	}	
}

	
	
void plot_Path(GtkWidget *Map)
{
	GdkGC *Red_GC;
	GdkColor Red;


	//////////////////////Setup Color Red////////////////////////////////////
	   gdk_color_parse("Red",&Red);
	   gdk_colormap_alloc_color(gdk_colormap_get_system(),&Red, FALSE, TRUE);
	   Red_GC = gdk_gc_new(Map -> window);
	   gdk_gc_set_foreground(Red_GC,&Red);


	   gdk_draw_line(Map -> window, Red_GC, 300, 220,(int)PointX,(int) PointY);
	   
}
	

void
on_GUIWindow_destroy                   (GtkObject       *object,
                                        gpointer         user_data)
{

}


void
on_MenuBar_destroy                     (GtkObject       *object,
                                        gpointer         user_data)
{

}


void
on_File_activate                       (GtkMenuItem     *menuitem,
                                        gpointer         user_data)
{
	g_print("FILE AcTIVEATE");
}


void
on_Exit_activate                       (GtkMenuItem     *menuitem,
                                        gpointer         user_data)
{
		gtk_main_quit();
}


void
on_exit1_activate                      (GtkMenuItem     *menuitem,
                                        gpointer         user_data)
{
		gtk_main_quit();
}

gboolean
on_RobotMap_event                      (GtkWidget       *widget,
                                        GdkEvent        *event,
                                        gpointer         user_data)
{
	return(FALSE);
}



gboolean
on_RobotMap_expose_event               (GtkWidget       *widget,
                                        GdkEventExpose  *event,
                                        gpointer         user_data)
{
	DrawingSurface = widget;
	draw_Grid(DrawingSurface,0,0);
	return(TRUE);
}


gboolean
on_RobotMap_configure_event            (GtkWidget       *widget,
                                        GdkEventConfigure *event,
                                        gpointer         user_data)
{
	Net.runMe();
	IncomingCommand = new RobotCommand;
        return TRUE;
	
}



void
on_Start_clicked                       (GtkButton       *button,
                                        gpointer         user_data)
{
	StartClicked = 1;
	scroll_map();
}



void
on_Faster_clicked                      (GtkButton       *button,
                                        gpointer         user_data)
{
	if(scrollrate != 120)
	{
		scrollrate += 5;
	}	
	draw_Grid(DrawingSurface,0,0);
	
}


void
on_Slower_clicked                      (GtkButton       *button,
                                        gpointer         user_data)
{
	if(scrollrate != -120)
	{
		scrollrate -= 5;
	}
		draw_Grid(DrawingSurface,0,0);
}

void on_Map_Clicked			(GtkWidget 	*Window,
					 GdkEventButton *event,
					 GtkWidget	*Map)
{
	PointX = (int)(event -> x);
	PointY = (int)(event -> y);
	draw_Grid(Window, 0,0);
}

void modify_Path(float xdirection, float ydirection)
{
	float XCheck;
	float YCheck;
	YCheck = PointY;
	XCheck = PointX;
	if(PointX > 300  && PointY < 220)
	{		
		PointX -= fabs(xdirection); 
		PointY += fabs(ydirection);
		if(PointX < 300 || PointY > 220)
		{
			PointX = 300;
			PointY = 220;
		}
	}
	if(PointX > 300 && PointY > 220)
	{
		PointX -= fabs(xdirection);
		PointY -= fabs(ydirection);
		if(PointX < 300 || PointY < 220)
		{
			PointX = 300;
			PointY = 220;
		}
	}
	if(PointX < 300 && PointY < 220)
	{
		PointX += fabs(xdirection);
		PointY += fabs(ydirection);
		if(PointX > 300 || PointY > 220)
		{
			PointX = 300;
			PointY = 220;
		}
	}
	if(PointX < 300 && PointY > 220)
	{
		PointX += fabs(xdirection);
		PointY -= fabs(ydirection);
		if(PointX > 300 || PointY < 220)
		{
			PointX = 300;
			PointY = 220;
		}
	}
		
}



void on_Key_Press(GtkWidget *Widget, GdkEventKey *event)
{
	char Key;
	Key = event -> keyval;

	if(Key == 'w' || Key == 'W' || Key == 'R')
	{
		scrollrate += 5;
		Net.changeSpeed(scrollrate);
		scroll_map();
		
	}
	
	if(Key == 'a' || Key == 'A' || Key == 'Q')
	{
		TurnRate -= 5;
		Net.steerMotor(TurnRate);
		scroll_map();
	}
	
	if(Key == 'S' || Key == 's' || Key == 'T')
	{
		scrollrate -= 5;
		Net.changeSpeed(scrollrate);
		scroll_map();
	}

	if(Key == 'd' || Key == 'D' || Key == 'D')
	{
		TurnRate += 5;
		Net.steerMotor(TurnRate);
		scroll_map();
	}

	if(Key == 'z' || Key == 'Z')
	{
		scrollrate = 0;
		StartClicked = 0;
		TurnRate = 0;
		scroll_map();
		Net.steerMotor(TurnRate);
		Net.changeSpeed(scrollrate);
	}
	if(Key == 'o' || Key == 'O')
	{
		TurnRate = 0;
		scroll_map();
		Net.steerMotor(TurnRate);
	}
	if(Key == 'q' || Key == 'Q')
	{
		scrollrate = 0;
		scroll_map();
		Net.changeSpeed(scrollrate);
	}
		
}