def vacconfirm():
	esp.signals.vacuumraw()
	esp.signals.vacuumraw()
	esp.signals.vacuumraw()
	esp.signals.vacuumraw()
	if esp.signals.vacuumraw() < 100:
		return 2
	else:
		return 1	


def finddrop():
	if esp.arm.move("drop") <0:
		print("Failed to find drop")
		if esp.arm.move("drop") <0:
			print("Double failed to find drop")
			return 1
	return 2


def findtube(tube):
	if esp.cylinder.tube(tube)<0:
		if esp.cylinder.tube(tube)<0:
			return 1
	return 2


def grabatcyl():
	if esp.arm.move("pick")<0:
		return 1
	esp.actuator.on(8)
	esp.actuator.off(9)
	esp.actuator.on(0)
	utime.sleep(2)
	if vacconfirm() == 1:
		esp.cylinder.jog(800)
		utime.sleep(1)
		if vacconfirm() == 1:
			esp.cylinder.jog(-1200)
			utime.sleep(1)
			if vacconfirm()<0:
				esp.actuator.off(0)
				esp.actuator.off(8)
				esp.arm.move("drop")
				esp.arm.release()
				esp.cylinder.release()
				print("Failed to reach Vacuum")
				return 1
	return 2


def armtofilter():	
	if esp.arm.move("filter")<0:
		return 1
	esp.arm.jog(75)
	return 2


def systemready():
	readyState=0
	while 1:
		if readyState==0:
			readyState += vacconfirm()
		elif readyState==1:  #no vacuum present
			print("state={}".format(readyState))
			readyState += esp.clamp.atOpen()
		elif readyState==2:  #vacuum present
			armoptos=esp.signals.armoptos()
			if armoptos==0x8:  #arm at pick position
				readyState==100
			elif armoptos==0xc: #arm at 
				readyState==200
			elif armoptos==0x4:
			elif armoptos==0:
			else:
				print("Arm optos bad")
				return 1

				esp.actuator.off(9)
				esp.actuator.on(8)
				esp.actuator.on(0)
				utie.sleep(1)
				if esp.clamp.fullopen()<0:
					print("Clamp failed to fully open")
					return 1
				esp.clamp.release()
				if esp.arm.move("drop")<0:
					print("Arm failed to reach drop")
					return 1
				esp.arm.jog(200)
				esp.actuator.on(9)
				utime.sleep(1)
				esp.actuator.off(9)
				esp.actuator.off(8)
				esp.actuator.off(0)
				esp.arm.release()
				return 2
		elif readyState == 1: 
			print("state={}".format(readyState))
			readyState += (esp.clamp.atClose() + 1)
		elif readyState == 2:  #clamp is open!
			print("state={}".format(readyState))
			print("Clamp is OPEN")
			return 2
		elif readyState == 3:  #clamp not at close
			print("state={}".format(readyState))
			readyState += (esp.clamp.aboveRetain() + 1)
		elif readyState ==4:  #clamp at close
			print("state={}".format(readyState))
			print("moving clamp up to retain")
			if esp.clamp.retain()<0:
				print("clamp failed to reach retain")
				return 1
			readyState=0
		elif readyState == 5: #clamp is below low retain
			print("state={}".format(readyState))
			readyState += (esp.clamp.belowRetain() +1)
		elif readyState == 6:  #clamp is above retain and not close
			print("state={}".format(readyState))
			print("moving clamp up to retain 2")
			if esp.clamp.retain()<0:
				print("clamp failed to reach retain")
				return 1
			readyState=0
		elif readyState == 7: #Clamp is well above upper retain
			print("state={}".format(readyState))
			print("try to grab puck and dispose")
			esp.arm.move("drop")
			esp.arm.jog(-1500)
			esp.actuator.on(0)
			esp.actuator.on(8)
			esp.actuator.off(9)
			esp.arm.jog(-50)
			utime.sleep(1)
			if vacconfirm() > 0:
				esp.signals.forceraw()
				esp.signals.forceraw()
				esp.signals.forceraw()
				if esp.clamp.fullopen()<0:
					print("Clamp failed to fully open")
					return 1
				esp.clamp.release()
				if esp.arm.move("drop")<0:
					print("Arm failed to reach drop")
					return 1
				esp.arm.jog(200)
				esp.actuator.on(9)
				utime.sleep(1)
				esp.actuator.off(9)
				esp.actuator.off(8)
				esp.actuator.off(0)
				esp.arm.release()
				return 2
			print("failed to acquire puck")				
			esp.actuator.on(9)
			utime.sleep(1)
			esp.actuator.off(9)
			esp.actuator.off(8)
			esp.actuator.off(0)
			esp.arm.release()
			return 1
		elif readyState == 8:  #Clamp is below upper retain
			print("state={}".format(readyState))
			print("moving clamp down to grab")
			if esp.clamp.grab()<0:
				print("clamp failed to grab")
				if esp.clamp.fullopen()<0:
					print("clamp failed to open")
					return 1
				return 2
			readyState=0
		else: 
			return 1
	return 1


def loadpuck(tube, startstate=0):
	loadstate=startstate
	while loadstate<100000:
		if loadstate==0:
			loadstate += finddrop()
		elif loadstate==1:
			print("Arm failed to find drop")
			return -2
		elif loadstate==2:
			loadstate += findtube(tube)
		elif loadstate==3:
			print("Cylinder failed to find {}".format(tube))
			return -3
		elif loadstate==4:
			loadstate += grabatcyl()
		elif loadstate==5:
			print("Failed to Grab puck at {}".format(tube))
			return -4
		elif loadstate==6:
			loadstate += armtofilter()
		elif loadstate==7:
			return -5
		elif loadstate==8:
			print("puck in position")
			esp.cylinder.release()
			esp.clamp.grab()
			esp.actuator.off(0)
			esp.actuator.on(9)
			utime.sleep(1)
			esp.actuator.off(9)
			esp.actuator.off(8)
			esp.arm.jog(200)
			esp.arm.release()
			esp.clamp.close()
			esp.clamp.release()
			return loadstate
		else:
			 return -1
	return loadstate


def unloadpuck():
	esp.clamp.retain()
	utime.sleep(1)
	esp.actuator.on(8)
	esp.actuator.on(0)
	esp.arm.jog(-300)
	utime.sleep(2)
	if vacconfirm() > 0:
		esp.arm.jog(100)
		esp.signals.forceraw()
		esp.signals.forceraw()
		esp.signals.forceraw()
		esp.clamp.open()
		esp.clamp.release()
		if esp.arm.move("drop")<0:
			return -5
		esp.arm.jog(200)
		esp.actuator.on(9)
		utime.sleep(1)
		esp.actuator.off(9)
		esp.actuator.off(8)
		esp.actuator.off(0)
		esp.arm.release()
		return 0
	else:
		esp.actuator.off(0)
		esp.actuator.off(8)
		esp.actuator.off(9)
		esp.arm.move("drop")
		esp.arm.release()
		esp.clamp.release()
		print("Failed to reach Vacuum")
		return -1