from SimpleCV import *
from nxt import *
import time
import string
import numpy as np

###FIND BRICK
print "Please turn on NXT. Press enter to continue"
raw_input()

brick = None
while(brick is None):
	print "Finding NXT..."
	try:
		brick = locator.find_one_brick(silent=True)
	except:
		print "Brick not found, check the connection and press enter to retry"
		raw_input()
print "Brick found!"

###DEFINE MOTORS
mConvey = Motor(brick, PORT_A)
mPlate = Motor(brick, PORT_B)
mTray = Motor(brick, PORT_C)

mConvey.idle()
mPlate.idle()
mTray.idle()

###RESET POSITION OF MOTOR A
mPlate.weak_turn(-60, 270)
time.sleep(0.5)
while(True):
	strRate = mPlate.get_tacho().__str__()
	time.sleep(0.2)
	NStrRate = mPlate.get_tacho().__str__()
	rate = int(strRate[1:string.find(strRate,",")])
	nRate = int(NStrRate[1:string.find(NStrRate,",")])
	rate = nRate - rate
	print rate
	if abs(rate) < 1: break

mPlate.idle()

###INITALISE CAMERA FEED
print "Which camera would you like to use?"
cam = raw_input()
cam = Camera(int(cam), {"width": 1920, "height": 1080})
disp = Display()
crop = (500,305,890,500)
n = 0
while n < 10:
	img = cam.getImage()
	img.save(disp)
	n += 1
n = 0
while n < 10:
	img = cam.getImage()
	img.drawRectangle(*crop,width=5)
	img.save(disp)
	n += 1
n = 0
while n < 10:
	img = cam.getImage().crop(*crop)
	img.save(disp)
	n += 1

#START SORTING
print "Let's start sorting!"

offset = 25
mPlate.turn(60, offset) #plate offset
time.sleep(3)

print "Calibrating..."
meanList = []
n=0

while n < 5:
	previous = cam.getImage().crop(*crop)
	time.sleep(0.05)
	current = cam.getImage().crop(*crop)
	diff = current - previous
	matrix = diff.getNumpy()
	mean = matrix.mean()
	meanList.append(mean)
	n +=1
	diff.save(disp)
mean = np.mean(meanList)
print mean
print meanList

if 2*mean < 0.7:	threshold = 2*mean
elif mean==0:	threshold = 0.5
else:	threshold = 0.7
print threshold

threshold = 2
mConvey.run(80, True)
while True:
	previous = cam.getImage().crop(*crop)
	time.sleep(0.05)
	current = cam.getImage().crop(*crop)
	diff = current - previous
	matrix = diff.getNumpy()
	mean = matrix.mean()
	diff.save(disp)
	if mean > threshold:
		mConvey.brake()
		break
time.sleep(1)
mPlate.turn(-40, offset)
time.sleep(1)
img = cam.getImage().crop(*crop)
img.save(disp)


time.sleep(5)
mConvey.idle()
mPlate.idle()
mTray.idle()