#------IMPORTS-------
from gestalt import nodes
from gestalt import interfaces
from gestalt import machines
from gestalt import functions
from gestalt.machines import elements
from gestalt.machines import kinematics
from gestalt.machines import state
from gestalt.utilities import notice
from gestalt.publish import rpc	#remote procedure call dispatcher
import time


#------VIRTUAL MACHINE------
class virtualMachine(machines.virtualMachine):
	
	def initInterfaces(self):
		if self.providedInterface: self.fabnet = self.providedInterface		#providedInterface is defined in the virtualMachine class.
		else: self.fabnet = interfaces.gestaltInterface('FABNET', interfaces.serialInterface(baudRate = 115200, interfaceType = 'ftdi', portName = '/dev/ttyUSB0'))
		
	def initControllers(self):
		self.x1AxisNode = nodes.networkedGestaltNode('X1 Axis', self.fabnet, filename = '086-005a.py', persistence = self.persistence)
		self.x2AxisNode = nodes.networkedGestaltNode('X2 Axis', self.fabnet, filename = '086-005a.py', persistence = self.persistence)
		self.y1AxisNode = nodes.networkedGestaltNode('Y1 Axis', self.fabnet, filename = '086-005a.py', persistence = self.persistence)
		self.y2AxisNode = nodes.networkedGestaltNode('Y2 Axis', self.fabnet, filename = '086-005a.py', persistence = self.persistence)

		self.x1x2Node = nodes.compoundNode(self.x1AxisNode, self.x2AxisNode)
		self.y1y2Node = nodes.compoundNode(self.y1AxisNode, self.y2AxisNode)
		
		self.xyNode = nodes.compoundNode(self.x1AxisNode, self.x2AxisNode, self.y1AxisNode, self.y2AxisNode)

	def initCoordinates(self):
		self.position = state.coordinate(['mm','mm','mm', 'mm'])
	
	def initKinematics(self):
		self.x1Axis = elements.elementChain.forward([elements.microstep.forward(4), elements.stepper.forward(1.8), elements.leadscrew.forward(8), elements.invert.forward(False)])
		self.x2Axis = elements.elementChain.forward([elements.microstep.forward(4), elements.stepper.forward(1.8), elements.leadscrew.forward(8), elements.invert.forward(False)])
		self.y1Axis = elements.elementChain.forward([elements.microstep.forward(4), elements.stepper.forward(1.8), elements.leadscrew.forward(8), elements.invert.forward(False)])
		self.y2Axis = elements.elementChain.forward([elements.microstep.forward(4), elements.stepper.forward(1.8), elements.leadscrew.forward(8), elements.invert.forward(False)])

		self.stageKinematics = kinematics.direct(4)	#direct drive on all axes
	
	def initFunctions(self):
		self.move = functions.move(virtualMachine = self, virtualNode = self.xyNode, axes = [self.x1Axis, self.x2Axis, self.y1Axis, self.y2Axis], kinematics = self.stageKinematics, machinePosition = self.position,planner = 'null')
		self.jog = functions.jog(self.move)	#an incremental wrapper for the move function
		pass
		
	def initLast(self):
#		self.machineControl.setMotorCurrents(aCurrent = 0.8, bCurrent = 0.8, cCurrent = 0.8)
#		self.xyzNode.setVelocityRequest(0)	#clear velocity on nodes. Eventually this will be put in the motion planner on initialization to match state.
		pass
	
	def publish(self):
#		self.publisher.addNodes(self.machineControl)
		pass
	
	def getPosition(self):
		return {'position':self.position.future()}
	
	def setPosition(self, position  = [None, None, None]):
		self.position.future.set(position)

	def setSpindleSpeed(self, speedFraction):
#		self.machineControl.pwmRequest(speedFraction)
		pass

#------IF RUN DIRECTLY FROM TERMINAL------
if __name__ == '__main__':
	# variables 
	speed = 3

	foamcutter = virtualMachine(persistenceFile = "test.vmp")

	# foamcutter.x1AxisNode.setMotorCurrent(0.7)

	foamcutter.x1x2Node.setVelocityRequest(speed)
	foamcutter.y1y2Node.setVelocityRequest(speed)
	foamcutter.xyNode.setVelocityRequest(speed)

	# # moves to test with
	#moves = [[140,0,140,0], [0, 0, 0, 0]]
	#moves = [[0, 0, 100, 100], [100, 100, 100, 100], [100, 100, 0, 0], [0, 0, 0, 0]]
#	moves =[[100,100,0,0], [100,100,5,5], [105,105,5,5], [105,105,10,10], [110,110,10,10], [110,110,15,15],[0,0,0,0]]
	moves =[[100,100,0,0], [100,100,10,10], [110,110,10,10], [110,110,20,20], [120,120,20,20], [120,120,30,30],[130,130,30,30],[130,130,60,60]]
	
	for move in moves:
		foamcutter.move(move, 0)
		status = foamcutter.x1AxisNode.spinStatusRequest()
		# This checks to see if the move is done.
		while status['stepsRemaining'] > 0:
			time.sleep(0.001)
			status = foamcutter.x1AxisNode.spinStatusRequest()