#!/usr/bin/env python
PKG = 'rospy_robot_debug' # this package name
import rospy
import nxt.locator
from nxt.sensor import *
from nxt.motor import PORT_A, PORT_B, PORT_C
import nxt.motor 

import threading

import roslib
import numpy as np
from math import *
import sys,time
import os
from std_msgs.msg import String

def test_touchsensors(b):
	print 'Touch:',
	if Touch(b, PORT_1).get_sample():
		print 'yes'
		return 'Touch yes'
        else:
		print 'no'
		return 'Touch no'
def test_motorsensors(b,iPower,iType):
	if iType==0:
		nxt.motor.Motor(b, PORT_A).run(iPower,False)
		nxt.motor.Motor(b, PORT_B).run(iPower,False)
	if iType==1:
		nxt.motor.Motor(b, PORT_A).run(-1*iPower,False)
		nxt.motor.Motor(b, PORT_B).run(-1*iPower,False)
		
def test_ultrasonicsensors(b):
	print 'Test range:',
        print Ultrasonic(b, PORT_2).get_sample()

def test_colorsensors(b):

        cs = Color20(b, PORT_3)
        print "This is the color sensor test make sure that your color sensor is plugged into port 1:"
	print 'RED:'
        cs.set_light_color(Type.COLORRED)
        time.sleep(1.0)
	print 'BLUE:'
        cs.set_light_color(Type.COLORBLUE)
        time.sleep(1.0)
	print 'GREEN:'
        cs.set_light_color(Type.COLORGREEN)
        time.sleep(1.0)
	print 'FULL:'
        cs.set_light_color(Type.COLORFULL)
        time.sleep(1.0)
	print 'OFF:'
        cs.set_light_color(Type.COLORNONE)
        print "INTENSITY READING:"
        start =rospy.Time.now()
        while rospy.Time.now()<start+rospy.Duration(5.0):
          print 'INTENSITY:', cs.get_reflected_light(Type.COLORBLUE)
          time.sleep(0.1)
        print "COLOR READING:"
        start =rospy.Time.now()
        while rospy.Time.now()<start+rospy.Duration(5.0):
          print 'COLOR:', cs.get_color()
          time.sleep(0.1)
	cs.set_light_color(Type.COLORNONE)
#============================================================================================

def soundcallback(data):
    rospy.loginfo(rospy.get_caller_id()+"Robot heard command: %s",data.data)
    #os.system("rosrun sound_play say.py %s"%data.data)    
def listener():
    rospy.Subscriber("/recognizer/output", String, soundcallback)
    rospy.spin()
    
    
def talker():
    pub = rospy.Publisher('LegsInfo', String)
    r = rospy.Rate(10) 
    while not rospy.is_shutdown():
        str = "Object distance :%d"%Ultrasonic(b, PORT_2).get_sample()
        rospy.loginfo(str)
        pub.publish(str)
        r.sleep()
#============================================================================================

def thread_main(a):
    global count, mutex
    threadname = threading.currentThread().getName()
    mutex.acquire()
    if a==0:	
    #'usb=True, bluetooth=False, fantomusb=True'	
	#sock = nxt.locator.find_one_brick(None, 'ROS1', False, None, False, None, None)
	sock = nxt.locator.find_one_brick(None, 'ROS1', False, None, False, {False,True,False,False}, None)	
    if a==1:
	#sock = nxt.locator.find_one_brick(None, 'ROS2', False, None, False, None, None)
	sock = nxt.locator.find_one_brick(None, 'ROS2', False, None, False, {False,True,False,False}, None)
    mutex.release()
    if sock:
	nxt.motor.Motor(sock, PORT_A).brake()
	nxt.motor.Motor(sock, PORT_B).brake()
	test_touchsensors(sock)
	if a==0:
		test_motorsensors(sock,80,0)
	if a==1:
		test_motorsensors(sock,80,1)
	#test_colorsensors(sock)
	#test_ultrasonicsensors(sock)	
	#nxt.motor.Motor(sock, PORT_A).brake()
	nxt.motor.Motor(sock, PORT_A).brake()
	nxt.motor.Motor(sock, PORT_B).brake()
		
    else:
    	if a==0:		     
		print 'No ROS1 NXT bricks found'
    	if a==1:		     
		print 'No ROS2 NXT bricks found'
    print threadname, a

if __name__ == '__main__':
	rospy.init_node('debug_robot',anonymous=True)

 	global count, mutex
 	threads = []     
 	mutex = threading.Lock()
 	for x in xrange(0, 2):
        	threads.append(threading.Thread(target=thread_main, args=(x,)))
    	for t in threads:
        	t.start()
    	for t in threads:
        	t.join()
#	os.system("roscore")        	
#	os.system("roslaunch pocketsphinx robocup.launch")
	os.system("rosrun sound_play say.py Start")
	rospy.sleep(3)
	listener()