# THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
# APPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
# HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
# OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
# PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
# IS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
# ALL NECESSARY SERVICING, REPAIR OR CORRECTION.

# IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
# WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
# THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
# GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
# USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
# DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
# PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
# EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
# SUCH DAMAGES.

# Copyright (c) 2018 Dr. Oliver Barth <info@drb-electronic.de>
# Interface to a kinematic (not part of basic package)

from KinematicSocketClient import *
from KinematicCommands import *
import time

Kinematic_keywords = [
	'self.kin.connect(server, port)',
	'self.kin.close()',
	'self.kin.isConnected()',
	'self.kin.enable()',
	'self.kin.disable()',
	'self.kin.stop()',
	'self.kin.resetAllMotors()',
	'self.kin.reset()',
	'self.kin.sendCommand(cmd)',
	'self.kin.isActive()',
	'self.kin.isInitialized()',
	'self.kin.waitTargetReached()',
	'self.kin.isTargetReached()',
	'self.kin.isEmergencyStopActive()',
	'self.kin.readActualAxisPosition()',
	'self.kin.readCommandAxisPosition()',
	'self.kin.readActualPosition()',
	'self.kin.readCommandPosition()',
	'self.kin.readActualAxisVelocity()',
	'self.kin.readMotionState()',
	'self.kin.getStateDrive()',
	'self.kin.moveDirectAbsolute(vel, pos, blocking=True)',
	'self.kin.moveDirectListPosition(vel, listPos, blocking=True)',
	'self.kin.moveDirectListName(vel, listName, blocking=True)',
	'self.kin.moveDirectPath(vel, listPosStart, listPosEnd, blocking=True)',
	'self.kin.moveLinearAbsolute(vel, pos, axispos, blocking=True)',
	'self.kin.moveLinearAbsoluteListPosition(vel, listPos, blocking=True)',
	'self.kin.moveLinearPath(iVelPercent, listPosStart, listPosEnd, blocking=True)',
	'self.kin.moveSplinePath(iVelPercent, iListPosStart, iListPosEnd)',
	'self.kin.startJogSingleAxis(num, vel)',
	'self.kin.stopJogSingleAxis()',
	'self.kin.startJogCartLin(iNrCtrl, iVelPercent, iTypeCoorsys)',
	'self.kin.stopJogCartLin()',
	'self.kin.home()',
	'self.kin.setDigitalOutput(port, num, on)',
	'self.kin.closeGripper()',
	'self.kin.openGripper()',
	'self.kin.openGripperToBase()',
	'self.kin.isGripperClosed()' ]

#
class Kinematic():
	#
	def __init__(self):
		self.sockClient = KinematicSocketClient()
		
	#
	# socket connection management
	#
	
	# tries to connect to motion control socket server 
	def connect(self, server, port):
		self.sockClient.connectToServer(server, port)
		time.sleep(0.5)
		return self.sockClient.isConnected()
	
	#
	def close(self):
		self.sockClient.close()
		
		#
	def isConnected(self):
		return self.sockClient.isConnected()

	# see plcopen motioncontrol: MC_GroupEnable
	def enable(self):
		self.sockClient.sendCmd(KinematicCommands.CMD_SYNCHALLMOT)
		
	# see plcopen motioncontrol: MC_GroupDisable
	def disable(self):
		self.sockClient.sendCmd(KinematicCommands.CMD_SYNCHALLMOT)
		
	# see plcopen motioncontrol: MC_GroupStop
	def stop(self):
		self.sockClient.sendCmd(KinematicCommands.CMD_STOP)

	#
	def resetAllMotors(self):
		self.sockClient.sendCmd(KinematicCommands.CMD_RESETALLMOT)

	# see plcopen motioncontrol: MC_Reset
	# This Function Block makes the transition from the state ErrorStop to Standstill
	# or Disabled by resetting all internal axis-related errors.
	def reset(self):
		self.sockClient.sendCmd(KinematicCommands.CMD_RESETEMSTOP)

	#
	def sendCommand(self, cmd):
		self.sockClient.sendCmd(cmd)

	#
	def isActive(self):
		return self.sockClient.isActive()

	#
	def isInitialized(self):
		return True

	#
	def waitTargetReached(self):
		while ((self.sockClient.getMotionState() != KinStateMotion.STATE_MOTION_IDLE) and self.sockClient.isConnected()):
			time.sleep(0.05)

	#
	def isTargetReached(self):
		if (self.sockClient.getMotionState() == KinStateMotion.STATE_MOTION_FINISHED):
			return True
		else:
			return False

	#
	def isEmergencyStopActive(self):
		if self.sockClient.isEMStop():
			return True
		else:
			return False

	#
	# read functions
	#  
	
	#
	def readActualAxisPosition(self):
		return self.sockClient.getActualAxisPosition()
	
	#
	def readCommandAxisPosition(self):
		return self.sockClient.getCommandAxisPosition()
	
	# see plcopen motioncontrol: MC_GroupReadActualPosition
	def readActualPosition(self):
		return self.sockClient.getActualPosition()
	
	#
	def readCommandPosition(self):
		return self.sockClient.getCommandPosition()
	
	#
	def readActualAxisVelocity(self):
		return self.sockClient.getActualAxisVelocity()
		
	# see plcopen motioncontrol: MC_ReadMotionState
	def readMotionState(self):
		return self.sockClient.getMotionState()
		
	#
	def getStateDrive(self):
		return self.sockClient.getDriveState()

	#
	# motion functions
	#
	
	# see plcopen motioncontrol: MC_MoveDirectAbsolute
	# vel				velocity
	# pos				target position[10]		
	def moveDirectAbsolute(self, vel, pos, blocking=True):
		initByVel = 1
		ret = self.sockClient.sendMoveDirectAbsolute(vel, pos, initByVel)
		if ret == 0:
			if blocking:
				self.waitTargetReached()
		return ret
			
	#
	def moveDirectListPosition(self, vel, listPos, blocking=True):
		initByVel = 1
		self.sockClient.sendMoveDirectListPosition(vel, listPos, initByVel)
		if blocking:
			self.waitTargetReached()
			
	#
	def moveDirectListName(self, vel, listName, blocking=True):
		initByVel = 1
		self.sockClient.sendMoveDirectListName(vel, listName, initByVel)
		if blocking:
			self.waitTargetReached()
			
	#
	def moveDirectPath(self, vel, listPosStart, listPosEnd, blocking=True):
		ret = self.sockClient.sendMoveDirectPath(vel, listPosStart, listPosEnd)
		if ret == 0:
			if blocking:
				self.waitTargetReached()
		return ret
			
	# see plcopen motioncontrol: MC_MoveLinearAbsolute
	# vel				path velocity in %
	# pos[6]			target position
	# axispos[10]		axis target used for aux axis 
	def moveLinearAbsolute(self, vel, pos, axispos, blocking=True):
		self.sockClient.sendMoveLinearAbsolute(vel, pos, axispos)
		if blocking:
			self.waitTargetReached()
	
	#
	def moveLinearAbsoluteListPosition(self, vel, listPos, blocking=True):
		self.sockClient.sendMoveToListPosCartLin(iVelPercent, listPos)
		if blocking:
			self.waitTargetReached()
	
	#
	def moveLinearPath(self, iVelPercent, listPosStart, listPosEnd, blocking=True):
		self.sockClient.sendMovePathListPosCartLin(iVelPercent, listPosStart, listPosEnd)
		if blocking:
			self.waitTargetReached()

	#
	# see plcopen motioncontrol: MC_MoveCircularAbsolute
	#
	
	#
	# move spline
	#
	
	#
	def moveSplinePath(self, iVelPercent, iListPosStart, iListPosEnd):
		self.sockClient.sendMovePathListPosSpline(iVelPercent, iListPosStart, iListPosEnd)
		self.waitTargetReached()

	#
	# jog functions
	#
		
	#
	def startJogSingleAxis(self, num, vel):
		self.sockClient.startJog(num, vel)

	def stopJogSingleAxis(self):
		self.sockClient.sendCmd(KinematicCommands.CMD_STOP_JOG_SINGLEAXIS)

	#
	def startJogCartLin(self, iNrCtrl, iVelPercent, iTypeCoorsys):
		self.sockClient.startJogCartLin(iNrCtrl, iVelPercent, iTypeCoorsys)
		self.waitTargetReached()

	#
	def stopJogCartLin(self):
		self.sockClient.sendCmd(KinematicCommands.CMD_STOP_JOG_LIN)

	#
	def home(self):
		self.sockClient.sendCmd(KinematicCommands.CMD_MOVE_HOME)
		
	#
	# process management
	#
	
	#
	def setDigitalOutput(self, port, num, on):
		self.sockClient.setDigitalOutput(port, num, on)

	#
	def closeGripper(self):
		self.sockClient.sendCmd(KinematicCommands.CMD_FIX_NEAREST_OBJECT_TO_TCP)
		while self.sockClient.getGripperState() != KinematicCommands.CMD_FIX_NEAREST_OBJECT_TO_TCP:
			time.sleep(0.05)

	#
	def openGripper(self):
		self.sockClient.sendCmd(KinematicCommands.CMD_RELEASE_FIXED_OBJECT)
		while self.sockClient.getGripperState() != KinematicCommands.CMD_RELEASE_FIXED_OBJECT:
			time.sleep(0.05)

	#
	def openGripperToBase(self):
		self.sockClient.sendCmd(KinematicCommands.CMD_FIX_OBJECT_TO_PLATFORM)
		while self.sockClient.getGripperState() != KinematicCommands.CMD_FIX_OBJECT_TO_PLATFORM:
			time.sleep(0.05)
			
			#
	def isGripperClosed(self):
		return PrjArmClientPython.StateArm_inst().m_bGripperClosed
		
	#
	def startWelding(self):
		self.sockClient.sendCmd(KinematicCommands.CMD_START_WELDING)
		
	#
	def stopWelding(self):
		self.sockClient.sendCmd(KinematicCommands.CMD_STOP_WELDING)
		
	#
	def startRotary(self, speed):
		port = 1000
		ispeed = int(speed * 1350)
		self.sockClient.setDigitalOutput(port, 0, ispeed)

	#
	def stopRotary(self):
		port = 1000
		self.sockClient.setDigitalOutput(port, 0, 0)

	#
	def startTool(self):
		port = 2000
		self.sockClient.setDigitalOutput(port, 0, 1)

	#
	def stopTool(self):
		port = 2000
		self.sockClient.setDigitalOutput(port, 0, 0)

	#
	# position list
	#
	
	#
	def loadPositionList(self, name):
		ret = self.sockClient.sendLoadPositionList(name)
		return ret
		
	#
	# parameter management
	#
	
	#
	def requestParams(self):
		self.sockClient.requestParams()
		
	#
	def paramsAvailable(self):
		return self.sockClient.paramsAvailable()
		
	#
	def getParams(self):
		return self.sockClient.getParams()
	
	#
	# miscellaneous
	#
	
	#
	def linIpo(self, x0, x1, y0, y1, y):
		val = x0 + (x1 - x0) * (y - y0) / (y1 - y0)
		return val

	#
	def intersectSegSeg(self, s1_s, s1_t, s2_s, s2_t, sp):
		x1 = s1_s[0]
		x2 = s1_t[0]
		x3 = s2_s[0]
		x4 = s2_t[0]
		y1 = s1_s[1]
		y2 = s1_t[1]
		y3 = s2_s[1]
		y4 = s2_t[1]
		den = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
		if (den == 0.0):
			return False
		ua = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3);
		ua /= den;
		#if ((ua > 1) or (ua < 0)):
		#    return false
		ub = (x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3);
		ub /= den;
		if ((ub > 1) or (ub < 0)):
			return False
		sp[0] = x1 + ua * (x2 - x1)
		sp[1] = y1 + ua * (y2 - y1)
		return True