# Creates instructions for the robot to move 360 degrees around, make photos of the ball-plate and write the ball_colors to the database

import MySQLdb
import random
import serial
import time
from color_detection_v4 import DetectColor
from struct import pack

def sendBin( dec ):
    x = pack('i', dec)
    ser.write(x[1])
    ser.write(x[0])

#ser = serial.Serial('/dev/ttyACM0', 9600) 	# Change name if necessary. Find out name with ls /dev/tty* (1x without, 1x with USB connected)
ser = serial.Serial(port='/dev/ttyACM0',	# Change name if necessary. Find out name with ls /dev/tty* (1x without, 1x with USB connected)
                    baudrate=9600,
                    parity=serial.PARITY_NONE,
                    stopbits=serial.STOPBITS_ONE,
                    bytesize=serial.EIGHTBITS,
                    timeout=1)
time.sleep(2) 								# Arduino resets, so wait for some time

db = MySQLdb.connect("localhost","root","root","rpr_robot" )	# Open database connection
cursor = db.cursor()											# prepare a cursor object using cursor() method

# Get the number of rows in the database for the loop
cursor.execute('select * from ball_positions')
cursor.fetchall()
num_entries = cursor.rowcount - 1

# Define the possible colors of the balls
colors = ["blue", "red", "yellow", "cyan", "green"]
is_empty = 0;

# Create instructions to turn around
signature = chr(0b01010010)     	# from raspberry
priority0 = chr(0b00000000)     	# add to end of queue
priority1 = chr(0b00000001)
moveJ = chr(0b00000001)          	# binary 1
moveA = chr(0b00000100)         	# binary 4
pause = chr(0b00000111)         	# binary 7
dontChange = chr(0b01111111) + chr(0b11111111)
eot = chr(0b11111110) +  chr(0b11111110)          # end of transmission
magOff = chr(0b00000000)
magOn = chr(0b00000001)
angle = (0, 45, 90, 135, 180, 225, 270, 315)

# Predefine messages
msgPause = signature + priority0 + pause
msgTurn = signature + priority0 + moveA

# Initialize color detection
ballColors = DetectColor()

# TODO: Check if robot is in save mode or move it to!


# Set startposition
ser.write(msgTurn)
sendBin(0)	# Keep R1 as it is
sendBin(110)			# Put P to this height
sendBin(90)				# Put R2 to this angle
ser.write(eot)
print("TO START")

time.sleep(10)

for i in angle:
	ser.write(msgTurn)
	sendBin(i)			# Turn R1 to actual angle
	ser.write(dontChange)	# Keep P as it is
	ser.write(dontChange)	# Keep R2 as it is
	ser.write(eot)
	print("TO "+str(i))
	
	ser.write(msgPause)
	sendBin(2000)			# Send Wait for 2 seconds
	ser.write(eot)
	print("pause at "+str(i))

	# TODO Wait time or wait for "I'm there" from robot
	time.sleep(3)
	i = i + 90
	if i >= 360:
            i = i - 360
	ballColors.startDetection(i)
	# TODO Wait time
	time.sleep(3)
	
ser.write(msgTurn)
sendBin(0)	# Keep R1 as it is
sendBin(110)			# Put P to this height
sendBin(90)				# Put R2 to this angle
ser.write(eot)
print("TO START")

# Go through array and write detected positions to database
for i in range(0, num_entries + 1):
	# Ball-Position 29 must be empty at any time!
	if i == 29 or ballColors.Matrix[i][1] == -1:
		is_empty = 1
		color = "invalid"
	else:
		is_empty = 0
		# Set random color -> Just for offline-programming!
                #color = random.choice(colors);
                color = colors[int(ballColors.Matrix[i][1])];

	
	# Prepare SQL query to UPDATE required records
	sql = "UPDATE ball_positions SET pos_isEmpty = %i, ball_color = '%s', ball_targetPos = 0, ball_prio = -1 WHERE pos_id = %i" % (is_empty, color, i)
	try:
		cursor.execute(sql)		# Execute the SQL command
		db.commit()				# Commit your changes in the database
	except:
		print "Error: unable to update records"
		db.rollback()			# Rollback in case there is any error
		
db.close()		# disconnect from server


# Only for debugging
while True:
    print(ser.readline())
	
file = open("testfile.txt", "w")
file.write(ballColors.Matrix);
file.close
#print ballColors.Matrix


ser.close()		# close serial connection