# 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)

import socket
from threading import Thread
from KinematicCommands import *
import time
import struct
from Queue import *
from plotXY import *
import xml.etree.ElementTree as ET

#
class KinematicSocketClient():
	#
	def __init__(self):
		self.connected = False
		self.msgQueue = Queue()
		self.axisPosAct = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
		self.axisVelAct = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
		self.axisPosCmd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
		self.posAct = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
		self.posCmd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
		self.driveState = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
		self.stateRespond = False
		self.motionState = KinStateMotion.STATE_MOTION_IDLE
		self.gripperState = KinematicCommands.CMD_RELEASE_FIXED_OBJECT
		self.digin = [0, 0, 0, 0]
		self.anin = [0, 0, 0, 0, 0, 0, 0, 0]
		self.response = 0
		self.paramList = []
		self.paramListUpdated = False
		
	#
	# socket connection management
	#
	   
	# 
	def connectToServer(self, server, port):
		self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
		self.socket.connect((server, port))
		self.socket.settimeout(0.0)
		self.connected = True
		self.sendCmd(KinematicCommands.CMD_FORCE_CONNECT)
		self.stateRespond = False
		thrd = Thread(target=self.task)
		thrd.start()
		waitCycles = 0
		while self.stateRespond == False:
			time.sleep(0.05)
			waitCycles = waitCycles + 1
			if waitCycles > 20:
				self.connected = False
				break
		
	#
	def close(self):
		self.sendCmd(KinematicCommands.CMD_DISCONNECT)
		time.sleep(0.5)
		self.connected = False
		self.socket.close()
	
	#
	def isConnected(self):
		return self.connected
		    
	#
	def sendCmd(self, command):
		s = struct.Struct('!B')
		msg = s.pack(command)
		self.msgQueue.put(msg)
		
	#
	# get functions
	#
	
	#
	def getActualAxisPosition(self):
		return self.axisPosAct[:]
	
	#
	def getCommandAxisPosition(self):
		return self.axisPosCmd[:]
	
	#
	def getActualPosition(self):
		return self.posAct[:]
		
	#
	def getCommandPosition(self):
		return self.posCmd[:]
	
	#
	def getActualAxisVelocity(self):
		return self.axisVelAct
		
	#
	def getMotionState(self):
		return self.motionState
	
	#
	def getDriveState(self):
		return self.driveState
		
	#
	# functions motion direct (point to point)
	#
	
	# 
	def sendMoveDirectAbsolute(self, vel, pos, initByVel):
		msgData = (KinematicCommands.CMD_MOVETO_PTP, vel, pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6], pos[7], pos[8], pos[9], initByVel)
		s = struct.Struct('!B d d d d d d d d d d d i')
		msg = s.pack(*msgData)
		self.response = -1
		self.msgQueue.put(msg)
		self.__waitResponse()
		return self.response
		
	#
	def sendMoveDirectListPosition(self, vel, listPos, initByVel):
		msgData = (KinematicCommands.CMD_MOVETO_LISTPOS_PTP, vel, listPos, initByVel)
		s = struct.Struct('!B d i i')
		msg = s.pack(*msgData)
		self.response = -1
		self.msgQueue.put(msg)
		self.__waitResponse()
		return self.response
		
	#
	def sendMoveDirectListName(self, vel, listPosName, initByVel):
		msgData = (KinematicCommands.CMD_MOVETO_LISTPOSNAME_PTP, vel, len(listPosName), listPosName, initByVel)
		s = struct.Struct('!B d i %ds i' % len(listPosName))
		msg = s.pack(*msgData)
		self.response = -1
		self.msgQueue.put(msg)
		self.__waitResponse()
		return self.response
		
	#
	def sendMoveDirectPath(self, vel, listPosStart, listPosEnd):
		msgData = (KinematicCommands.CMD_MOVEPATH_LISTPOS_PTP, vel, listPosStart, listPosEnd)
		s = struct.Struct('!B d i i')
		msg = s.pack(*msgData)
		self.response = -1
		self.msgQueue.put(msg)
		self.__waitResponse()
		return self.response
		
	#
	# functions motion linear
	#
		
	# 
	def sendMoveLinearAbsolute(self, vel, pos, axispos):
		msgData = (KinematicCommands.CMD_MOVETO_LIN, vel,
				pos[0], pos[1], pos[2], pos[3], pos[4], pos[5],
				axispos[0], axispos[1], axispos[2], axispos[3], axispos[4],
				axispos[5], axispos[6], axispos[7], axispos[8], axispos[9])
		s = struct.Struct('!B d d d d d d d d d d d d d d d d d')
		msg = s.pack(*msgData)
		self.msgQueue.put(msg)
		
	#
	# functions motion circular
	#
	
	#
	# functions motion spline
	#
		
	# load position list file at server and download to this client
	# return	0 on success 
	def sendLoadPositionList(self, name):
		msgData = (KinematicCommands.CMD_LOAD_AND_DOWNLOAD_LIST, len(name), name)
		s = struct.Struct('!B i %ds' % len(name))
		msg = s.pack(*msgData)
		self.response = -1
		self.msgQueue.put(msg)
		self.__waitResponse()
		return self.response
		
	#
	# parameter management
	#
	
	#
	def requestParams(self):
		self.paramListUpdated = False
		self.sendCmd(KinematicCommands.CMD_GET_PARAMS)

	#
	def paramsAvailable(self):
		return self.paramListUpdated
	
	#
	def getParams(self):
		self.paramListUpdated = False
		return self.paramList

	#
	def getGripperState(self):
		return self.gripperState
	
	#
	def setDigitalOutput(self, port, num, on):
		msgData = (KinematicCommands.CMD_SETDIGOUT, port, num, on)
		s = struct.Struct('!B i i i')
		msg = s.pack(*msgData)
		self.msgQueue.put(msg)
	
	#
	def isEMStop(self):
		return False
	
	#
	def startJog(self, num, vel):
		msgData = (KinematicCommands.CMD_START_JOG_SINGLEAXIS, num, vel)
		s = struct.Struct('!B i d')
		msg = s.pack(*msgData)
		self.msgQueue.put(msg)
		
	#
	def __waitResponse(self):
		waitCycles = 0
		while self.response == -1:
			time.sleep(0.05)
			waitCycles = waitCycles + 1
			if waitCycles > 40:
				break;

	# adds header and length to message and sends it out
	def __send(self, msg):
		headerData = (0xFF, 0x01, 0xFE, 0x02, len(msg))
		s = struct.Struct('!B B B B i')
		header = s.pack(*headerData)
		try:
			self.socket.send(header)
			self.socket.send(msg)
		except socket.error, e:
			self.connected = False
		 
	# running cyclically to
	# - send out messages from queue
	# - request state
	# - receive and evaluate messages
	def task(self):
		cnt = 0
		while(self.connected):
			# send
			if self.msgQueue.empty() == False:
				msg = self.msgQueue.get() 
				self.__send(msg)
			 # receive
			try:
				rxData = self.socket.recv(65536)
				length = len(rxData)
				start = 0
			except socket.timeout, e:
				pass
			except socket.error, e:
				pass
			else:
				while length > 8:
					# header
					rxHeder = rxData[0 + start:4 + start]
					
					#  message length (without header and length)
					rxDataLength = rxData[4 + start:8 + start]
					s = struct.Struct('!i')
					rxLength = s.unpack(rxDataLength)[0] + 8
					
					# command byte
					rxDataCommand = rxData[8 + start:9 + start]
					s = struct.Struct('B')
					rxCommand = s.unpack(rxDataCommand)
		
					if rxCommand[0] == KinematicCommands.CMD_GETSTATE:
						rxDataSize = rxData[9 + start:13 + start]
						s = struct.Struct('!i')
						rxSize = s.unpack(rxDataSize)[0]
						sdata = rxData[13 + start:rxSize + 13 + start]
						tree = ET.ElementTree(ET.fromstring(sdata))
						node = tree.find('meas_axis_position')
						for i in range(0, 10):
							self.axisPosAct[i] = float(node.get('a' + str(i)))
							
						node = tree.find('meas_axis_velocity')
						for i in range(0, 10):
							self.axisVelAct[i] = float(node.get('a' + str(i)))
						
						node = tree.find('cmd_axis_position')
						for i in range(0, 10):
							self.axisPosCmd[i] = float(node.get('a' + str(i)))
						
						node = tree.find('frame_meas')
						names = ['x', 'y', 'z', 'a', 'b', 'c']
						for i in range (0, 6):
							self.posAct[i] = float(node.get(names[i]))
							
						node = tree.find('frame_cmd')
						for i in range (0, 6):
							self.posCmd[i] = float(node.get(names[i]))
							
						for i in range (0, 10):
							node = tree.find('drive_status' + str(i))
							self.driveState[i] = int(node.get('drv'))
						
						node = tree.find('system_status')
						self.motionState = int(node.get('motion'))
						self.stateRespond = True
					elif rxCommand[0] == KinematicCommands.CMD_GET_PARAMS:
						rxDataSize = rxData[9 + start:13 + start]
						s = struct.Struct('!i')
						rxSize = s.unpack(rxDataSize)[0]
						sdata = rxData[13 + start :rxSize + 13 + start]
						tree = ET.ElementTree(ET.fromstring(sdata))
						self.paramList = []
						for param in tree.findall('param'):
							key = param.get('key')
							descr = param.get('descr')
							data_type = param.get('data_t')
							access_type = param.get('access_t')
							group = param.get('group')
							value = param.get('val')
							list_entry = { 'key':key, 'descr':descr, 'data_type':data_type, 'access_type':access_type, 'group':group, 'value':value }
							self.paramList.append(list_entry)
						self.paramListUpdated = True
					elif rxCommand[0] == KinematicCommands.CMD_LOAD_AND_DOWNLOAD_LIST:
						rxError = rxData[9 + start:13 + start]
						self.response = struct.Struct('!i').unpack(rxError)[0]
					elif (rxCommand[0] == KinematicCommands.CMD_MOVETO_PTP) \
						or (rxCommand[0] == KinematicCommands.CMD_MOVETO_LISTPOS_PTP) \
						or (rxCommand[0] == KinematicCommands.CMD_MOVETO_LISTPOSNAME_PTP) \
						or (rxCommand[0] == KinematicCommands.CMD_MOVEPATH_LISTPOS_PTP) \
						or (rxCommand[0] == KinematicCommands.CMD_MOVETO_LIN) :
						rxError = rxData[9 + start:13 + start]
						self.response = struct.Struct('!i').unpack(rxError)[0]
					else:
						pass
					length = length - rxLength
					if length > 8:
						start = start + rxLength				
			# request    
			cnt = cnt + 1
			if (cnt > 3) and self.connected:
				self.sendCmd(KinematicCommands.CMD_GETSTATE)
				cnt = 0            
			time.sleep(0.01)