#!/usr/bin/env python
import time
import math
import numpy

from pyscope import tecnai


def goto(t,xy,z,a=0.0):
	xydict = {'x':xy[0]*1e-6,'y':xy[1]*1e-6,'z':z*1e-6}
	xydict['a'] = a*math.pi/180.0
	print 'going to', xy, z, a
	t.setStagePosition(xydict)
	final = t.getStagePosition()
	print 'got to  ', '(%.2f,%.2f)  %.2f %.2f' % (final['x']*1e6,final['y']*1e6, final['z']*1e6, final['a']*180.0/math.pi)
	time.sleep(1)

def getTopsLows(tem, xys, thresholds, z, tops, lows):
	low_trip_value = thresholds[0]
	high_trip_value = thresholds[1]
	for i in range(len(xys)):
		print 'loop', i
		goto(tem,xys[i],z,)
		time.sleep(2)
		current = tem.getScreenCurrent()
		print current*1e11
		if current > high_trip_value:
			tops[int(current*1e14)] = xys[i]
			break
		if current > low_trip_value:
			tops[int(current*1e14)] = xys[i]
			if len(tops.keys()) == 3:
				break
		lows[int(current*1e14)] = xys[i]
	return tops, lows

if __name__ == '__main__':
	tem = tecnai.EFKrios()
	'''
	tem.setColumnValvePosition('closed')
	tem.setMainScreenPosition('down')
	current0 = tem.getScreenCurrent()
	print current0*1e11
	'''
	tem.setStagePosition({'a':0.0})
	tem.setColumnValvePosition('open')

	current0 = 0.0
	thresholds = [3.5e-11, 12e-11]
	d = 400
	z = 45e-6
	tops = {}
	lows = {}
	xys = [(d,0),(0,0),(0,d),(-d,0),(0,-d)]
	tops, lows = getTopsLows(tem, xys, thresholds, z, tops, lows)
	if len(tops) < 3 and len(lows) >= 3:
		best_low = lows[max(lows.keys())]
		worst_low = lows[min(lows.keys())]
		g = (best_low[0]-worst_low[0],best_low[1]-worst_low[1])
		matrix = numpy.array([[g[0],g[1]],[g[1],-g[0]]])
		rot_mat = math.sqrt(2)*matrix / numpy.linalg.norm(matrix)
		xys = [(1.4*d,-1.4*d),(2*d,0),(1.4*d,1.4*d),(0,2*d),(-1.4*d,1.4*d),(-2*d,0),(-1.4*d,-1.4*d),(0,-2*d)]
		xys = numpy.array(xys)
		rotxys = []
		for i in range(xys.shape[0]):
			rotxys.append(rot_mat.dot(xys[i,:]))
		tops, lows = getTopsLows(tem, rotxys, thresholds, z, tops, lows)

	print tops
	best = tops[max(tops.keys())]
	goto(tem,best,z,)
	pos = tem.getStagePosition()
	current = tem.getScreenCurrent()
	print pos, current
	raw_input('enter to quit')