# 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>
# Frame for controlling motion functionality (not part of basic package)

import Tkinter as tk
import ttk
from tkFileDialog import askopenfilename
from drbLayout import *
from motionFile import *
from decimal import Decimal
from drbTable import *

MotionModes = [ "PointToPoint", "Linear", "Spline" ]
StateMotion = [ "STARTUP", "IDLE", "EXT_CONTROL", "JOG_SINGLE_JOINT", "JOG_JOINT", "JOG_FRM", "PTP", "CART_LINEAR",
	"CART_CIRCULAR", "CART_SPLINE", "STOP", "QUICK_STOP", "CHANGE_TCP", "FINISHED"]
MotionFileDir = "../drb_motion/Bin/Arm/MotionFiles"

#
#
#
class FrameMotion():
	#
	def __init__(self, frm, kin):
		
		self.kin = kin
		self.posOffset = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
		
		self.MAX_AXIS = 6
		self.entFrmAct = []
		self.labAxisNum = []
		self.entPosAct = []
		self.butJog = []
		self.entDriveStatus = []
		
		varJogVel = tk.DoubleVar()
		varMoveVel = tk.DoubleVar()
		
		actRow = 0
			
		self.scaleJogVel = tk.Scale(frm, showvalue=1, variable = varJogVel, from_=0, to=100, orient=tk.HORIZONTAL)
		self.scaleJogVel.grid(column=4, row=actRow, columnspan=2, sticky="ew", padx=3, pady=3)
		actRow = actRow + 1
		
		frmTxt = ['X', 'Y', 'Z', 'A', 'B', 'C']
		for i in range (0, self.MAX_AXIS):
			lab = ttk.Label(frm, text="a" + str(i + 1) , padding=1)
			lab.grid(column=0, row=actRow + i, sticky="nsew", padx=1, pady=1)
			self.labAxisNum.append(lab);
			
			ent = ttk.Entry(frm, width=10)
			ent.grid(column=1, row=actRow + i, sticky="nsew", padx=1, pady=1)
			self.entFrmAct.append(ent)

			lab = ttk.Label(frm, text=frmTxt[i] , padding=1)
			lab.grid(column=2, row=actRow + i, sticky="nsew", padx=1, pady=1)
			self.labAxisNum.append(lab);
			
			ent = ttk.Entry(frm, width=10)
			ent.grid(column=3, row=actRow + i, sticky="nsew", padx=1, pady=1)
			self.entPosAct.append(ent);
			
			but = ttk.Button(frm, text='jog A' + str(i + 1) + '+' , width=10)
			but.bind('<ButtonPress-1>', lambda event, a=i, b=1:self.startJog(a, b))
			but.bind('<ButtonRelease-1>', lambda event, a=i, b=0:self.stopJog(a, b))
			but.grid(column=4, row=actRow + i, sticky="nsew", padx=1, pady=1)
			self.butJog.append(but)
			
			but = ttk.Button(frm, text='jog A' + str(i + 1) + '-' , width=10)
			but.bind('<ButtonPress-1>', lambda event, a=i, b=-1:self.startJog(a, b))
			but.bind('<ButtonRelease-1>', lambda event, a=i, b=0:self.stopJog(a, b))
			but.grid(column=5, row=actRow + i, sticky="nsew", padx=1, pady=1)
			self.butJog.append(but)
			
			ent = ttk.Entry(frm, width=20)
			ent.grid(column=6, row=actRow + i, columnspan=1, sticky="nsew", padx=1, pady=1)
			self.entDriveStatus.append(ent)
		actRow = 7
		
		self.labState = ttk.Label(frm, text="state", padding=1)
		self.labState.grid(column=0, row=actRow, sticky="ew", padx=1, pady=1)
		self.entState = ttk.Entry(frm, width=10)
		self.entState.grid(column=1, row=actRow, sticky="nsew", padx=1, pady=1)
		actRow = actRow + 1
		
		self.posTableRowKey = 0
		self.entryFilename = ttk.Entry(frm, width=10)
		self.entryFilename.grid(column=0, row=actRow, columnspan=7, sticky="nsew", padx=3, pady=3)
		actRow = actRow + 1
		
		# position table
		self.posTableFrame = ttk.Frame(frm, relief=tk.SUNKEN)
		self.posTableFrame.grid(column=0, row=actRow, columnspan=7, rowspan=4, sticky="nsew", padx=1, pady=1)
		colWidth = [10, 10, 10, 10, 10, 10, 10]
		colHeader = ['name', 'x', 'y', 'z', 'a', 'b', 'c']
		self.posTable = DrbTable(self.posTableFrame, "pos", 1, 7, colWidth, colHeader)

		self.butInsert = ttk.Button(frm, text='insert', padding=0, width=10, command=self.insert)
		self.butInsert.grid(column=8, row=actRow, sticky="nsew", padx=1, pady=1)
		self.butDelete = ttk.Button(frm, text='delete', padding=0, width=10, command=self.delete)
		self.butDelete.grid(column=8, row=actRow + 1, sticky="nsew", padx=1, pady=1)
		self.butLoadList = ttk.Button(frm, text='load', padding=0, width=10, command=self.load)
		self.butLoadList.grid(column=8, row=actRow + 2, sticky="nsew", padx=1, pady=1)
		self.butSaveList = ttk.Button(frm, text='save', padding=0, width=10, command=self.save)
		self.butSaveList.grid(column=8, row=actRow + 3, sticky="nsew", padx=1, pady=1)
		
		self.comboMotionModes = ttk.Combobox(frm, values=MotionModes, width=10)
		self.comboMotionModes.grid(column=9, row=actRow, sticky="nsew", padx=1, pady=1)
		self.comboMotionModes.current(0)
		self.scaleMoveVel = tk.Scale(frm, showvalue=1, variable = varMoveVel, from_=0, to=100, orient=tk.HORIZONTAL)
		self.scaleMoveVel.grid(column=9, row=actRow + 1, sticky="nsew", padx=1, pady=1)
		self.butMoveTo = ttk.Button(frm, text='move', padding=0, width=10, command=self.move)
		self.butMoveTo.grid(column=9, row=actRow + 2, sticky="nsew", padx=1, pady=1)
		self.butStop = ttk.Button(frm, text='stop', padding=0, width=10, command=self.stop)
		self.butStop.grid(column=9, row=actRow + 3, sticky="nsew", padx=1, pady=1)
		actRow = actRow + 4
		
		self.butEditParam = ttk.Button(frm, text='read', padding=0, width=10, command=self.editParam)
		self.butEditParam.grid(column=0, row=actRow, sticky="nsew", padx=1, pady=1)
		
		self.dbg = 0

	#
	def startJog(self, numAxis, direction):
		if self.kin.isConnected() == True:
			vel = self.scaleJogVel.get()
			self.kin.startJogSingleAxis(numAxis, direction * vel);
		
	#
	def stopJog(self, numAxis, direction):
		if self.kin.isConnected() == True:
			self.kin.stopJogSingleAxis()
		
	#
	def insert(self):
		pos = ['']
		for i in range (0, self.MAX_AXIS):
			pos.append(self.entPosAct[i].get())
		self.posTable.addRow(pos)

	#
	def delete(self):
		self.posTable.delSelRow()
		
	#
	def move(self):
		if self.kin.isConnected() == True:
			row = self.posTable.getSelRow()
			vel = self.scaleMoveVel.get()
			pos = [Decimal(row[1]), Decimal(row[2]), Decimal(row[3]), Decimal(row[4]), Decimal(row[5]), Decimal(row[6]), 0.0, 0.0, 0.0, 0.0]
			self.kin.moveDirectAbsolute(vel, pos, False)
			
	#
	def stop(self):
		if self.kin.isConnected() == True:
			self.kin.stop()
			
	#
	def setPos(self):
		if self.kin.isConnected() == True:
			self.posOffset[0] = float(self.entPosAct[0].get()) - float(self.entSetA0.get()) + self.posOffset[0]
			self.posOffset[1] = float(self.entPosAct[1].get()) - float(self.entSetA1.get()) + self.posOffset[1]
			self.posOffset[2] = float(self.entPosAct[2].get()) - float(self.entSetA2.get()) + self.posOffset[2]
			
	#
	def load(self):
		self.posTable.clear()
		name = askopenfilename(initialdir=MotionFileDir)
		f = MotionFile()
		pl = []
		f.read(name, pl)
		for i in range(0, len(pl)):
			self.posTable.addRow( [pl[i]['name'], pl[i]['a1'], pl[i]['a2'], pl[i]['a3'],
				pl[i]['a4'], pl[i]['a5'], pl[i]['a6']] )
		self.entryFilename.delete(0, 'end')
		self.entryFilename.insert(0, name)

	#
	def save(self):
		cnt = self.posTable.getRowCount()
		pl = []
		for i in range(1, cnt):
			row = self.posTable.getRow(i)
			dat = {'name':row[0], 'a1':row[1], 'a2':row[2], 'a3':row[3], 'a4':row[4], 'a5':row[5], 'a6':row[6]}
			pl.append(dat)
		name = askopenfilename(initialdir=MotionFileDir)
		f = MotionFile()
		f.write(name, pl)
		
	#
	def editParam(self):
		self.top = tk.Toplevel()
		self.top.title("parameters");
		self.paramTableFrame = ttk.Frame(self.top)
		self.paramTableFrame.grid(column=0, row=0, columnspan=7, rowspan=4, sticky="nsew", padx=3, pady=3)
		colWidth = [10, 10, 10, 10, 10, 10]
		colHeader = [ 'idx', 'description', 'data_type', 'access_type', 'group', 'value' ]
		self.paramTable = DrbTable(self.paramTableFrame, "param", 1, 6, colWidth, colHeader)
		self.butReadParam = ttk.Button(self.top, text='read', padding=0, width=10, command=self.readParam)
		self.butReadParam.grid(column=8, row=1, sticky="nsew", padx=1, pady=1)
		self.butLoadParam = ttk.Button(self.top, text='write', padding=0, width=10, command=self.writeParam)
		self.butLoadParam.grid(column=8, row=2, sticky="nsew", padx=1, pady=1)
		return

	#
	def readParam(self):
		self.kin.requestParams()

	#
	def writeParam(self):
		return
			
	#
	def driveStateToString(self, s):
		ret = ''
		if (s & 0x00200000) != 0:
			ret = ret + 'LIMSW_HARD_MAX | '
		if (s & 0x00400000) != 0:
			ret = ret + 'LIMSW_HARD_MIN | '
		if (s & 0x00800000) != 0:
			ret = ret + 'LIMSW_SOFT_MAX | '
		if (s & 0x01000000) != 0:
			ret = ret + 'LIMSW_SOFT_MIN'
		return ret

	#
	def update(self):
		pos = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
		if self.kin.isConnected() == True:
			pos = self.kin.readActualAxisPosition()
			tcp = self.kin.readActualPosition()
			state = self.kin.readMotionState()
			driveState = self.kin.getStateDrive()
			for i in range (0, self.MAX_AXIS):
				self.entPosAct[i].delete(0, "end")
				spos = format(pos[i]- self.posOffset[i], '.4f')
				self.entPosAct[i].insert(0, spos)
				
				self.entDriveStatus[i].delete(0, "end")
				st = self.driveStateToString(driveState[i])
				self.entDriveStatus[i].insert(0, st)
			
			self.entState.delete(0, "end")
			self.entState.insert(0, StateMotion[state])
			
			for i in range (0, 6):
				self.entFrmAct[i].delete(0, "end")
				self.entFrmAct[i].insert(0, format(tcp[i], '.3f'))

		paramList = []
		if self.kin.paramsAvailable() == True:
			#cnt = self.paramTableModel.getRowCount()
			#self.paramTableModel.deleteRows(range(0, cnt))
			paramList = self.kin.getParams()
			for i in range(0, len(paramList)):
				self.paramTable.addRow([paramList[i]['key'], paramList[i]['descr'], paramList[i]['data_type'],
					paramList[i]['access_type'], paramList[i]['group'], paramList[i]['value']] )

		return pos