import time

from Neobotix.Utils import Globalvars

class Gripper(object):


	def __init__(self, ArmObject):
		self.Arm = ArmObject
		self.constants = Globalvars.globalvars()
	
		pass

	#Gripper functions
	#close function
	def close(self, SizeObject, ClosePosition):
                if(self.Arm.getJointPos(6) == ClosePosition):
                        return True
 		#move to position
		self.Arm.moveJointPTP(self.constants.gripVel, True, 6, ClosePosition)
		#check position - if smaller: wrong cup, if bigger: error 
		currentJointPos = self.Arm.getJointPos(6)
		lastJointPos = 0
		startTime = time.time()
		#if Arm gripper reports same pos -> stop
		while ((not (currentJointPos == lastJointPos)) and ((time.time()-startTime) < self.constants.timeOut)):
			time.sleep(0.5)
			lastJointPos = currentJointPos
			currentJointPos = self.Arm.getJointPos(6)
			#print "curJointPos " + str(currentJointPos) + "openpos " + str(ClosePosition)
		#self.Arm.moveJointPTP(self.constants.gripVel, True, 6, currentJointPos)
		#self.Arm.waitTargetReached()		
		time.sleep(0.5)
		self.Arm.stopDrive(6)
		time.sleep(0.5)
		if (currentJointPos < SizeObject):
			print "Timeout. Object not grasped."
			return False
		else:
			print "Object grasped"
			return True


	def CloseRing(self):
		return self.close(self.constants.ringSize ,self.constants.closePosition)


	def CloseSmallCup(self):
		return self.close(self.constants.smallCupSize ,self.constants.closePosition)


	def CloseBigCup(self):
		return self.close(self.constants.bigCupSize, self.constants.closePosition)


	def openGripper(self, OpenPosition):
		if(self.Arm.getJointPos(6) == OpenPosition):
			return True
		self.Arm.moveJointPTP(self.constants.gripVel, True, 6, OpenPosition)
		##check position - if smaller: wrong cup, if bigger: error 
		currentJointPos = self.Arm.getJointPos(6)
		lastJointPos = 0
		startTime = time.time()
		#if Arm gripper reports same pos -> stop
		while ((not (currentJointPos == lastJointPos)) and ((time.time()-startTime) < self.constants.timeOut)):
			time.sleep(0.5)
			lastJointPos = currentJointPos
			currentJointPos = self.Arm.getJointPos(6)
			#print "curJointPos " + str(currentJointPos) + "openpos " + str(OpenPositionSmall)
		time.sleep(0.5)
		self.Arm.stopDrive(6)
		time.sleep(0.5)
		if (currentJointPos < (self.constants.openPositionSmall-0.005)):
			print "Timeout. Gripper crashed. "
			return False
		else:
			print "Gripper opened. "
			return True


	def OpenBigObject(self):
		return self.openGripper(self.constants.openPositionBig)


	def OpenSmallObject(self):
		return self.openGripper(self.constants.openPositionSmall)


	def Pour(self):
		self.Arm.sendCanMessage(232,self.constants.VibrateOn)
		self.Arm.moveJointPTP(self.constants.pourVel, True, 4, self.constants.Degree_90)
		self.Arm.waitTargetReached()
		time.sleep(10)
		#a=Arm.getTCPPos ()
		#Arm.moveCartLin( 40, a[0], a[1], a[2]-0.01, a[3], a[4]-0.6, a[5])
		#Arm.waitTargetReached()
		#Arm.moveCartLin( 40, a[0]-0.5, a[1], a[2]-0.01, a[3], a[4]-0.6, a[5])
		#Arm.waitTargetReached()
		#Arm.moveCartLin( 40, 0, 0, 0, 0, 0, Arm.constants.Degree_90)
		#Arm.waitTargetReached()
		self.Arm.moveJointPTP(self.constants.pourVel, True, 4, self.constants.Degree_120)
		self.Arm.waitTargetReached()
		time.sleep(2)
		self.Arm.sendCanMessage(232,self.constants.VibrateOff)