# A virtual machine for two axes
# Written to be called from mods.cba.mit.edu
# From e.g. the "fabnet.js" module
# Running the fabnetserver "fabserver.js"
# or from the commandline
# like python fabnet_xyaxes.py '[[[1,2],[0,0]]]'
#
# Users: set the portName, e.g. /dev/ttyUSB0
# Change the kinematics (incl direction) in initKinematics
# If you change which PCBs you use, delete the 
# virtual machine persistance file to start over (e.g. thismachine.vmp)
#
# Nadya Peek 2016


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


#------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):
		print "init controllers, x and y"
		self.xAxisNode = nodes.networkedGestaltNode('X Axis', self.fabnet, filename = '086-005a.py', persistence = self.persistence)
		self.yAxisNode = nodes.networkedGestaltNode('Y Axis', self.fabnet, filename = '086-005a.py', persistence = self.persistence)
                self.zAxisNode = nodes.networkedGestaltNode('Z Axis', self.fabnet, filename = '086-005a.py', persistence = self.persistence)
		self.aAxisNode = nodes.networkedGestaltNode('A Axis', self.fabnet, filename = '086-005a.py', persistence = self.persistence)
		self.xyzaNode = nodes.compoundNode(self.xAxisNode, self.yAxisNode, self.zAxisNode, self.aAxisNode)

	def initCoordinates(self):
		self.position = state.coordinate(['mm','mm','mm','mm'])
	
	def initKinematics(self):
		self.xAxis = elements.elementChain.forward([elements.microstep.forward(4), elements.stepper.forward(1.8), elements.leadscrew.forward(8), elements.invert.forward(False)])
		self.yAxis = elements.elementChain.forward([elements.microstep.forward(4), elements.stepper.forward(1.8), elements.leadscrew.forward(8), elements.invert.forward(True)])
                self.zAxis = elements.elementChain.forward([elements.microstep.forward(4), elements.stepper.forward(1.8), elements.leadscrew.forward(8), elements.invert.forward(True)])
		self.aAxis = 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.xyzaNode, axes = [self.xAxis, self.yAxis, self.zAxis, self.aAxis], 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.xNode.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]):
		self.position.future.set(position)

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

#------IF RUN DIRECTLY FROM TERMINAL------
if __name__ == '__main__':
	# The persistence file remembers the node you set. It'll generate the first time you run the
	# file. If you are hooking up a new node, delete the previous persistence file.


	# You can load a new program onto the nodes if you are so inclined. This is currently set to 
	# the path to the 086-005 repository on Nadya's machine. 
	#stages.xyNode.loadProgram('../../../086-005/086-005a.hex')
	
	# This is a widget for setting the potentiometer to set the motor current limit on the nodes.
	# The A4982 has max 2A of current, running the widget will interactively help you set. 
	#stages.xyNode.setMotorCurrent(0.7)

	# This is for how fast the motors move
	stages.xyzaNode.setVelocityRequest(4)	
	
	# Pull the moves out of the provided string
	# X axis, positive moves left max 300
	# Y axis, positive brings front max 300
	# Z axis, positive moves up max 300
	# A axis, positive rotates pump to the right, no max value. 
	# absolute coordinates, tell the location you want in the next movement not the change to location. 
	# to hold current position, you need to carry the location value to the next movement.
	# example ['0','0','0','0'],['20','0','0','0'],['0','20','0','0'],['0','20','20','0']
	# moves x left 20, moves Y front 20 while X returns to 0, moves Z up 20 while Y holds at 20
	#['x','y','z','a'] template for the axis figures. 
	#['0','0','0','0'],
	moves = [['0','0','0','0'],['0','-50','0','0'],['0','-50','-10','0'],['0','-50','-10','60'],['100','-50','-10','60'],['100','-50','-60','60'],['100','-50','-60','40'],['100','-50','-10','40'],['200','-50','-10','40'],['200','-50','-60','40'],['200','-50','-60','20'],['200','-50','-10','20'],['300','-50','-10','20'],['300','-50','-60','20'],['300','-50','-60','0'],['300','-50','-10','0'],['0','0','0','0']]
	#takes 60ml fluid, runs 3 well dips, and returns to home.
	#moves = [['-160','50','100','0']] #for quick return to home while testing.

	# Move!
	for move in moves:
		stages.move(move, 0)
		status = stages.xAxisNode.spinStatusRequest()
		# This checks to see if the move is done.
		while status['stepsRemaining'] > 0:
			time.sleep(0.001)
			status = stages.xAxisNode.spinStatusRequest()	
	

	stages = virtualMachine(persistenceFile = "fabnet2axes.vmp")