# -*- coding: utf-8 -*-
"""
Example to command EPOS4 with python
EPOS4 Operation Mode: Homing and Profile Position Mode

Testversion python 2.7
Tested under Ubuntu 16.04 and Raspberry Pi 3

Created on 09.03.2020
Version 1.0
@author: CCMC, maxon motor ag, Switzerland

"""

from ctypes import *
from time import sleep

##=====================================================================================================
# Folder created for example: /home/pi/src/python/
# Copy maxon motor Linux Library arm v7 into this folder
# Library must match according your cpu, eg. PI3 has arm v7
# EPOS Comand Library can be found here, when EPOS Studio has been installed:
# C:\Program Files (x86)\maxon motor ag\EPOS IDX\EPOS4\04 Programming\Linux Library
# path='/home/pi/src/python/libEposCmd.so.6.6.1.0'
path='/home/alg_sys/python/libEposCmd.so.6.6.1.0'
cdll.LoadLibrary(path)
epos=CDLL(path)

# Node ID must match with Hardware Dip-Switch setting of EPOS4
NodeID=4

# Global Variables
ret=0
keyhandle=0
pErrorCode=c_uint()

# Define States
States = ['OpenComPort', 'ConfHHM', 'ConfPPM', 'EnableDevice', 'Home', 'Move1', 'DisableDevice', 'CloseComPort', 'Error']
# Initial State
state = 'OpenComPort'


## States =====================================================================================================
def OpenPort(pErrorCode):
	print('NodeID: %d' % NodeID)
	# Open USB Port
	global keyhandle
	keyhandle=epos.VCS_OpenDevice('EPOS4', 'MAXON SERIAL V2', 'USB', 'USB0', byref(pErrorCode) )
		
	if keyhandle!=0:
		print('Keyhandle: %8d' % keyhandle)
		return 1
	
	else:
		print('Could not open Com-Port')
		ret = EvalError(state, pErrorCode)
		return 0


def ConfigHHM(pErrorCode):
	# Homing Parameters
	HomingAccleration=100
	SpeedSwitch=10
	SpeedIndex=10
	HomeOffset=0
	CurrentThreashold=200
	HomePosition=-333
	
	ret=epos.VCS_SetHomingParameter(keyhandle, NodeID, HomingAccleration, SpeedSwitch, SpeedIndex, HomeOffset, CurrentThreashold, HomePosition, byref(pErrorCode) )

	if ret==1:
		ret=EvalError(state, pErrorCode)
		#ToDo: Check, that HM is activated
		ret=epos.VCS_ActivateHomingMode(keyhandle, NodeID, byref(pErrorCode) )
		print('Homing Activated')
		return 1
		
	else:
		print('Error SetHomingParameter')
		return 0


def ConfigPPM(pErrorCode):
	ProfileVelocity=500
	Acceleration=2000
	Decleration=2000
		
	ret=epos.VCS_SetPositionProfile(keyhandle, NodeID, ProfileVelocity, Acceleration, Decleration, byref(pErrorCode) )
	
	if ret==1:	
		ret=EvalError(state, pErrorCode)
		return 1
		
	else:
		print('Error SetPositionProfile')
		return 0


def EnableDevice(pErrorCode):
	plsEnabled=c_bool()
	
	# Check Device Error before enabling
	ret=DeviceError(pErrorCode)
	
	if ret==1:
		# Enable Device
		ret=epos.VCS_SetEnableState(keyhandle, NodeID, byref(pErrorCode) )
		if ret==1:
			ret=EvalError(state, pErrorCode)
			if ret==1:
				# Check if Device is Enabled
				ret=epos.VCS_GetEnableState(keyhandle, NodeID, byref(plsEnabled), byref(pErrorCode))
						
				if plsEnabled.value==1:
					print('Device Enabled')
					return 1
		
				else:
					print('Device Not Enabled!')
					return 0
					
			else:
				return 0
				
		else:
			print('Error during enabling Device')
			return 0
	
	# Device is in Error State
	else:
		return 0


def Homing(pErrorCode):
	pPositionIs=c_long()
	
	# 35: Actual Position 
	# 34: Index Positive Speed
	# 33: Index Negative Speed
	# 27: Home Switch Negative Speed
	# 23: Home Switch Positive Speed
	# more details see "EPOS Command Library"
	HomingMethode = 34
	
	# Start Homing
	ret=epos.VCS_FindHome(keyhandle, NodeID, HomingMethode, byref(pErrorCode) )
	
	if ret==1:
		ret=EvalError(state, pErrorCode)
		print('Homing...')
		
		#Wait finishing Homing
		TimeOut=10
		ret=HomingAttained(TimeOut, pErrorCode)
		if ret==1:
			# Read Position Actual Value after Homing
			ret=epos.VCS_GetPositionIs(keyhandle, NodeID, byref(pPositionIs), byref(pErrorCode) )
			print('Actual Position after Homing: %8d [qc]' % pPositionIs.value)
			
			#ToDo: Check, that PPM is activated
			ret=epos.VCS_ActivateProfilePositionMode(keyhandle, NodeID, byref(pErrorCode) )
			print('Profile Position Mode activated')
			
			return 1
			
		else:
			return 0

	else:
		print('Error FindHome')
		return 0


def Move1(pErrorCode):
	# Parameters PPM
	# Position given in [qc]
	TargetPosition=50000
	# RelativeMovement=1
	AbsolutMovement=0
	StartProfileImmediately=1
	
	# Start Movement
	ret=epos.VCS_MoveToPosition(keyhandle, NodeID, TargetPosition, AbsolutMovement, StartProfileImmediately, byref(pErrorCode) )
	
	if ret==1:
		ret=EvalError(state, pErrorCode)
		print('Wait finishing positioning...')
		
		# Wait for target reached, time out in seconds
		TimeOut=10
		ret=PositionReached(TimeOut, pErrorCode)
		if ret==1:
			return 1
		else:
			return 0
		
	else:
		print('Error MoveToPosition')
		return 0


def DisableDevice(pErrorCode):
	plsDisabled=c_bool()
	
	ret=epos.VCS_SetDisableState(keyhandle, NodeID, byref(pErrorCode) )
	
	if ret==1:
		ret=EvalError(state, pErrorCode)
		if ret==1:
			ret=epos.VCS_GetDisableState(keyhandle, NodeID, byref(plsDisabled), byref(pErrorCode))
			if plsDisabled.value==1:
				print('Device Disabled')
				return 1
				
			else:
				print('Device could not be disabled')
				return 0
				
		else:
			return 0
			
	else:
		print('Error SetDisableState')
		return 0


def CloseComPort(pErrorCode):
	ret=epos.VCS_CloseDevice(keyhandle, byref(pErrorCode) )
	if ret==1:
		ret=EvalError(state, pErrorCode)
		return 1
		
	else:
		print('Error CloseDevice')
		return 0


## Functions ====================================================================================================
# Error of Command Execution
def EvalError(state, pErrorCode):
	pErrorInfo=' ' * 40
	
	if pErrorCode.value != 0x0:
		epos.VCS_GetErrorInfo(pErrorCode.value, pErrorInfo, 40)
		print('Error Code %#5.8x in State: %s' % (pErrorCode.value, state) )
		print('Description: %s' % pErrorInfo)
		return 0
	
	# No Error
	else:
		return 1


# Check Error State of EPOS4
def DeviceError(pErrorCode):
	pDeviceErrorCode=c_uint()
	
	ret=epos.VCS_GetDeviceErrorCode(keyhandle, NodeID, 1, byref(pDeviceErrorCode), byref(pErrorCode) )
	
	if pDeviceErrorCode.value==0:
		ret=EvalError(state, pErrorCode)
		return 1
		
	else:
		print('Device Error: %#5.8x' % pDeviceErrorCode.value )
		print('For more information see Firmware Specification')
		return 0


# Check for Target Reached
def PositionReached(iTimeOut, pErrorCode):
	pTargetReached=c_bool()
	i=0
	
	while True:
		# Timed out, Target not reached within time
		if i >= iTimeOut:
			print('Target Reached Timed Out')
			return 0
			break
			
		# Get Target Reached Bit of Statusword 0x6064
		ret=epos.VCS_GetMovementState(keyhandle, NodeID, byref(pTargetReached), byref(pErrorCode) )
		if ret==1:
			# Check Error Code of Command
			ret=EvalError(state, pErrorCode)
			if ret==1:
				if pTargetReached.value==1:
					print('Target Reached')
					return 1
					break
					
				# Target not Reached
				else:
					i+=1
					sleep(1)
			
			# Error in Commanding
			else:
				print('Error Commanding')
				return 0
				break
		
		# Command not executed
		else:
			print('Error Command GetMovementState')
			return 0
			break
	


# Wait for Homing to be finish	
def HomingAttained(iTimeOut, pErrorCode):
	pHomingAttained=c_bool()
	pHomingError=c_bool()
	i=0
	while True:
		# Timed out, Target not reached within time
		if i >= iTimeOut:
			print('Homing Timed Out')
			return 0
			break
		
		# Read Status of Homing
		ret=epos.VCS_GetHomingState(keyhandle, NodeID, byref(pHomingAttained), byref(pHomingError), byref(pErrorCode) )
		if ret==1:
			# Check Error Code of Command
			ret=EvalError(state, pErrorCode)
			if ret==1:
				if pHomingAttained.value==1:
					print('Homing Attained')
					return 1
					break
					
				elif pHomingError.value==1:
					print('Homing Error')
					return 0
					break
					
				# Homing not finished
				else:
					i+=1
					sleep(1)
			
			# Error in Commanding
			else:
				print('Error Commanding')
				return 0
				break
		
		# Command not executed
		else:
			print('Error GetHomingState')
			return 0
			break



def OtherFunction():
	pass


## Main Loop =====================================================================================================
while True:

	if state=='OpenComPort':
		print('State: %s' % state)
		ret=OpenPort(pErrorCode)
		if ret==1:
			state='ConfHHM'
		else:
			break
	
	
	elif state=='ConfHHM':
		print('State: %s' % state)
		ret=ConfigHHM(pErrorCode)
		if ret==1:
			state='ConfPPM'
		else:
			state='Error'
	
	
	elif state=='ConfPPM':
		print('State: %s' % state)
		ret=ConfigPPM(pErrorCode)
		if ret==1:
			state='EnableDevice'
		else:
			state='Error'
	
	
	elif state=='EnableDevice':
		print('State: %s' % state)
		ret=EnableDevice(pErrorCode)
		if ret==1:
			state='Home'
		else:
			state='Error'
	
	elif state=='Home':
		print('State: %s' % state)
		ret=Homing(pErrorCode)
		if ret==1:
			state='Move1'
		else:
			state='Error'
	
	
	elif state=='Move1':
		print('State: %s' % state)
		ret=Move1(pErrorCode)
		if ret==1:
			state='DisableDevice'
		else:
			state='Error'
	
	
	elif state=='DisableDevice':
		print('State: %s' % state)
		ret=DisableDevice(pErrorCode)
		if ret==1:
			state='CloseComPort'
		else:
			state='Error'
			
	
	elif state=='CloseComPort':
		print('State: %s' % state)
		ret=CloseComPort(pErrorCode)
		break


	elif state=='Error':
		# Do some action e.g. Reset Device e.t.c
		print('State: %s' % state)
		print('Execution Error: Exit Programm')
				
		state='CloseComPort'