#!/usr/bin/env python2
# -*- coding: latin-1 -*-

#import os
#import errno
import serial
import sys
import time
import QboCmd

port = '/dev/serial0'
move_to = [0]
touch_str = ''

try:

	print(port)

	# Open serial port
	ser = serial.Serial(port, baudrate=115200, bytesize=serial.EIGHTBITS, stopbits=serial.STOPBITS_ONE, parity=serial.PARITY_NONE, rtscts=False, dsrdtr=False, timeout=0)

	print "Open serial port sucessfully."
	print(ser.name)

	QBO = QboCmd.Controller(ser)
	
except:
	print "Error opening serial port."
	sys.exit()


def WaitForTouch():
	

	touch_str = ""
	touch = QBO.GetHeadCmd("GET_TOUCH", 0)

	if touch:
		global touch_str

		if touch == [1]:
			move_to = [1]
			touch_str = "Touch: right"
			print("Right Positon")
			print("Blue Nose")
			QBO.SetNoseColor(1) 			
			
		elif touch == [2]:
			touch_str = "Touch: up"
			print("Center head")
			print("Cyan Nose")
			QBO.SetNoseColor(5)
			print("Smile")
			QBO.SetMouth(0x110E00) #happy
			QBO.SetServo(1,500, 100)#Axis,Angle,Speed
			#Pause
			time.sleep(1)
			print("Switch off")
			QBO.SetNoseColor(0)
			
		elif touch == [3]:
			touch_str = "Touch: left"
			print("Left position")
			print("Green Nose")
			QBO.SetNoseColor(6) 
			
		if touch == [1] or touch == [2] or touch == [3]:
			print(touch_str)
			#fifo = os.open(FIFO_feel, os.O_WRONLY)
			#os.write(fifo, touch_str)
		
	time.sleep(.250)
	return touch_str

def Move_Head():
	global touch_str
	
	#print("Head move_to............ ", touch_str)
	if touch_str == "Touch: right":
		#Move the head to the right if touch [1]
		print("Move right")
		QBO.SetServo(1,210, 100)#Axis,Angle,Speed
		time.sleep(1)
		print("Smile")
		QBO.SetMouth(0x110E00) #happy
		
		touch_str = 'ok'
	
	if touch_str == "Touch: left":
		#Move the head to the left if touch [3]
		print("Move left")
		QBO.SetServo(1,725, 100)#Axis,Angle,Speed
		#Pause
		time.sleep(1)
		print("Smile")
		QBO.SetMouth(0x110E00) #happy
		
		touch_str = 'ok' 	

def Move_ok():
	time.sleep(3)
	global touch_str
	if touch_str == "ok":
		print("Move center")
		QBO.SetServo(1,500, 100)#Axis,Angle,Speed
		time.sleep(3)
		print("by / by")
		QBO.SetMouth(0) #none
		time.sleep(1)
		QBO.SetNoseColor(0) # none
		touch_str = ''
		time.sleep(1)

while True:
	time.sleep(1)
	WaitForTouch()
	Move_Head()
	Move_ok()