import OSC, time, threading, os, serial
 
import pyfirmata
from pyfirmata import *
from boards import BOARDS
 
class ArduinoMega(Board):
	"""
	A board that will set itself up as an Arduino Mega.
	"""
	def __init__(self, *args, **kwargs):
		args = list(args)
		args.append(BOARDS['arduino_mega'])
		super(ArduinoMega, self).__init__(*args, **kwargs)
 
	def __str__(self):
		return 'Arduino Mega %s on %s' % (self.name, self.sp.port)
 
# Adjust that the port match your system:
PORT = '\\.\COM4' 		#On Windows
#PORT = '/dev/ttyACM0' 	#On RPi
 
# Creates a new board 
board = ArduinoMega(PORT)
 
# ##################################################################
def engine(path, tags, args, source):
	print path, "@TAGS", tags, "@ARGS", args, "@SOURCE", source
	file = "Car_Start_2.mp3"
	print "starting engine"
	#os.startfile(file) # 
 
# ##################################################################
def rcMode(path, tags, args, source):
	print path, "@TAGS", tags, "@ARGS", args, "@SOURCE", source
	print "Switching to RC mode"
	#os.startfile(file)
# ##################################################################
def autoMode(path, tags, args, source):
	print path, "@TAGS", tags, "@ARGS", args, "@SOURCE", source
	print "Switching to AUTO mode"
	#os.startfile(file)
# ##################################################################
def questMode(path, tags, args, source):
	print path, "@TAGS", tags, "@ARGS", args, "@SOURCE", source
	print "Switching to QUEST mode"
	#os.startfile(file)	
 
# LIGHTS SECTION
# ##################################################################
PIN2 = board.get_pin('d:2:p')
def lightR(path, tags, args, source):
	PIN2.write(1 - args[0])
	print path, "@TAGS", tags, "@ARGS", args, "@SOURCE", source	
# ##################################################################
# ##################################################################
PIN3 = board.get_pin('d:3:p')
def lightG(path, tags, args, source):
	PIN3.write(1- args[0])
	print path, "@TAGS", tags, "@ARGS", args, "@SOURCE", source	
# ##################################################################
# ##################################################################
PIN4 = board.get_pin('d:4:p')
def lightB(path, tags, args, source):
	PIN4.write(1-args[0])
	print path, "@TAGS", tags, "@ARGS", args, "@SOURCE", source	
# ##################################################################
# ##################################################################
PIN13 = board.get_pin('d:13:p')
def lightFloor(path, tags, args, source):
	PIN13.write(1 - args[0])
	print path, "@TAGS", tags, "@ARGS", args, "@SOURCE", source	
# ##################################################################
 
 
# MOTOR SECTION
# ##################################################################
PINA = board.get_pin('d:8:s')
PINB = board.get_pin('d:9:s')
PINC = board.get_pin('d:10:s')
PIND = board.get_pin('d:11:s')
 
def motor(aa,bb,cc,dd):
	print aa, bb, cc, dd
 
	if aa==1: PINA.write(20)
	if aa==-1: PINA.write(138)
	if aa==0: PINA.write(91)
 
	if bb==1: PINB.write(20)
	if bb==-1: PINB.write(138)
	if bb==0: PINB.write(91)
 
 
	if cc==1: PINC.write(20)
	if cc==-1: PINC.write(138)
	if cc==0: PINC.write(91)
 
	if dd==1: PIND.write(20)
	if dd==-1: PIND.write(138)
	if dd==0: PIND.write(91)
 
def xmotor(path, tags, args, source):
	direction = path.split("/")[2]
	print "PIN", path.split("/")[2], "@TAGS", 
	print tags, "@ARGS", args, "@SOURCE", source	
 
	if args[0]==0: 
		motor(0,0,0,0)
	else:
		if direction == "nn" 	: motor(1,0,-1,0)
		if direction == "ne"	: motor(1,1,-1,-1)
		if direction == "ee"	: motor(0,1,0,-1)
		if direction == "se"	: motor(-1,1,1,-1)
		if direction == "ss"	: motor(-1,0,1,0)
		if direction == "sw"	: motor(-1,-1,1,1)
		if direction == "ww"	: motor(0,-1,0,1)
		if direction == "nw"	: motor(1,-1,-1,1)
		if direction == "freeze": motor(0,0,0,0)
# ##################################################################	
# ##################################################################
 
def user_callback(path, tags, args, source):
	user = ''.join(path.split("/"))
	print "cb-->",  path, "@TAGS", tags, 
	print "@ARGS", args, "@SOURCE", source
 
def quit_callback(path, tags, args, source):
	#pass
	# don't do this at home (or it'll quit blender)
	global run
	run = False
 
# tupple with ip, port
receive_address = "192.168.0.47", 7110  
 
# OSC Server. there are three different types of server. 
#s = OSC.OSCServer(receive_address) # basic
s = OSC.ThreadingOSCServer(receive_address) # threading
##s = OSC.ForkingOSCServer(receive_address) # forking
 
s.addDefaultHandlers()
 
s.addMsgHandler( "/1/start", engine)
 
s.addMsgHandler( "/1/rc", rcMode) 	
s.addMsgHandler( "/1/auto", autoMode)
s.addMsgHandler( "/1/quest", questMode)
 
s.addMsgHandler( "/1/nn", xmotor)
s.addMsgHandler( "/1/ne", xmotor)
s.addMsgHandler( "/1/ee", xmotor)
s.addMsgHandler( "/1/se", xmotor)
s.addMsgHandler( "/1/ss", xmotor)
s.addMsgHandler( "/1/sw", xmotor)
s.addMsgHandler( "/1/ww", xmotor)
s.addMsgHandler( "/1/nw", xmotor)
s.addMsgHandler( "/1/freeze", xmotor)
 
s.addMsgHandler( "/2/red", lightR)
s.addMsgHandler( "/2/green", lightG)
s.addMsgHandler( "/2/blue", lightB)
 
s.addMsgHandler( "/2/grey", lightFloor)
 
s.addMsgHandler( "/ping", user_callback )
s.addMsgHandler( "/quit", quit_callback )
 
# define a message-handler function for the server to call.
def printing_handler(addr, tags, stuff, source):
	print "---"
	print "received new osc msg from %s" % OSC.getUrlStr(source)
	print "with addr : %s" % addr
	print "typetags %s" % tags
	print "data %s" % stuff
	print "---"
 
s.addMsgHandler("/print", printing_handler) # adding our function
 
 
# just checking which handlers we have added
print "Registered Callback-functions are :"
for addr in s.getOSCAddressSpace(): print addr
 
# Start OSCServer
print "\nStarting OSCServer. Use ctrl-C to quit."
st = threading.Thread( target = s.serve_forever )
st.start()
 
try :
	while 1 : pass
 
except KeyboardInterrupt :
	print "\nClosing OSCServer."
	s.close()
	print "Waiting for Server-thread to finish"
	st.join() ##!!!
	print "Done"