# Two stage example Virtual Machine file
# moves get set in Main
# usb port needs to be set in initInterfaces
# Nadya Peek Dec 2014

#------IMPORTS-------
# THE PICKER MACHINE : Our machine need to Pick and Cut A musicSheet based in 30 notes with 70 mm for works.
# The distance beetwen each notes are 2mm so the X Axis have to move there
# The punchHeader going DOWN/UP for make holes. Y Axis Moves.
# Finally cuts done the Z Axis, roll the MusicSheet every 4mm.
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

#JAVIBG INITIALIZE NOTES AND SONG ARRAYS
# x axis has 30 notes with 2mm of space between notes, UNITS: MM
x_notes = {
'C3': 0, 'D3': 2, 'G3': 4, 'A3': 6, 'B3': 8,
'C4': 10, 'D4': 12, 'E4': 14, 'F4': 16, 'Gb4': 18, 'G4': 20, 'Ab4': 22, 'A4': 24, 'Bb4': 26, 'B4': 28,
'C5': 30, 'Db5': 32, 'D5': 34, 'Eb5': 36, 'E5': 38, 'F5': 40, 'Gb5': 42, 'G5': 44, 'Ab5': 46, 'A5': 48, 'Bb5': 50, 'B5': 52,
'C6': 54, 'D6': 56, 'E6': 58
}

# y axis is the stepper motor that does the perforation it needs to move a full turn to do a perforation UNITS: rev
y_perforation = 1

# z axis is the roller motor, there is a 4mm space between each line of the song UNITS: MM
z_roller = 4

#  SECOND DEFINE AN ARRAY TO WRITE THE SONG, EACH VECTOR IS ONE LINE, THIS SONG IS C MAJOR SCALE AND C MAJOR CHORD
song = [['Bb5', 'Bb4', 'F5'], [], [], [], [], [], ['Bb4'], [], [], [], [], [], ['Bb4', 'F5', 'D5'], [], [], [], [], [], ['Bb4'], [], [], [], [], [], ['Eb5', 'Bb4'], [], ['D5'], [], ['C5'], [], ['Bb5', 'Bb4', 'F5'], [], [], [], [], [], ['Bb4'], [], [], [], [], [], ['Bb4', 'D5', 'F5'], [], [], [], [], [], ['Eb5', 'Bb4'], [], ['D5'], [], ['C5'], [], ['Eb5', 'Bb5', 'Bb4'], [], [], [], [], [], ['Bb4'], [], [], [], [], [], ['Bb4', 'D5', 'F5'], [], [], [], [], [], ['Eb5', 'C5'], [], ['B4', 'D5'], [], ['Eb5', 'C5'], [], ['A4', 'C5'], [], [], [], [], [], ['A4'], [], [], [], [], [], ['F4'], [], [], [], ['F4'], [], ['G4'], [], [], [], [], [], ['Bb4'], [], [], ['G4'], [], [], ['Eb5', 'Bb4'], [], [], ['D5'], [], [], ['C5'], [], [], ['Bb4'], [], [], ['Bb4'], [], ['C5'], [], ['D5'], [], ['C5'], [], [], [], ['G4'], [], ['A4'], [], [], [], [], [], ['F4'], [], [], [], ['F4'], [], ['G4'], [], [], [], [], [], ['Bb4'], [], [], ['G4'], [], [], ['Eb5', 'Bb4'], [], [], ['D5'], [], [], ['C5'], [], [], ['Bb4'], [], [], ['Db5', 'F5'], [], [], [], [], [], ['A4', 'C5'], [], [], [], [], [], ['F4'], [], [], [], [], [], ['C4'], [], [], ['F4'], [], [], [], ['G4'], [], [], [], [], [], ['Bb4'], [], [], ['G4'], [], [], ['Eb5', 'Bb4'], [], [], ['D5'], [], [], ['C5'], [], [], ['Bb4'], [], [], ['Bb4'], [], ['C5'], [], ['D5'], [], ['C5'], [], [], [], ['G4'], [], ['F4', 'A4'], [], [], [], [], [], ['F4', 'F5'], [], [], [], ['F5'], [], ['Bb5', 'Gb5'], [], [], [], ['F5', 'Ab5'], [], ['Eb5', 'Gb5'], [], [], [], ['Db5', 'F5'], [], ['Eb5', 'C5'], [], [], [], ['Bb4', 'Db5'], [], ['Ab4', 'C5'], [], [], [], ['Gb4', 'Bb4'], [], ['F4', 'F5']]


#------VIRTUAL MACHINE------ Main functions
class virtualMachine(machines.virtualMachine):

#------Interfaces functions for RS 485 Serial Port Communication.
def initInterfaces(self):
# We need to change the name of our FabNet usbPort here. In my case was : '/dev/tty.usbserial-FTZ29L92'
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'))
#------Nodes/drivers Function for Gestalt Nodes. I modified the basic xy_plotter example adding zAxisNode and compounds
def initControllers(self):

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.xyzNode = nodes.compoundNode(self.xAxisNode, self.yAxisNode, self.zAxisNode )
#------Function for state and position coord based in cartensian coordinates system, i added
def initCoordinates(self):

# JAVIBG Y MOTOR COORDINATE IN REVOLUTIONS
 self.position = state.coordinate(['mm', 'rev' , 'mm' ])

#------Kinematics function for storage the initial arguments for each motor . This function must to adjust testing each step for each axis .
#- I added the z Kinematics and the number for direct drive all axes to 3
 def initKinematics(self):

self.xAxis = elements.elementChain.forward([elements.microstep.forward(4), elements.stepper.forward(1.8), elements.leadscrew.forward(7), 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(False)])
self.zAxis = elements.elementChain.forward([elements.microstep.forward(4), elements.stepper.forward(1.8), elements.leadscrew.forward(8), elements.invert.forward(False)])
self.stageKinematics = kinematics.direct(3) #direct drive on all axes

 def initFunctions(self):

self.move = functions.move(virtualMachine = self, virtualNode = self.xyzNode, axes = [self.xAxis, self.yAxis, self.zAxis ] , 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.
	stages = virtualMachine(persistenceFile = "test.vmp")

	# 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
	stages.xyzNode.setVelocityRequest(8)
	# Some random moves to test with
	# Move!

    # JAVIBG
    # Movement is defined
    # 0. for each line in song
    # 1. position the z_roller
    # 2. if not empty then for each note
    # 3. move the first x_note in the row
    # 4. make the perforation with y_perforation one revolution
    # 5. go back to the x_note 0
    # 6. end for each note
    # 7. end for each line

    moves = [[0,0,0]]
    current_z = 0

    for line in song:
        if line:
            for note in line:
                aux = [x_notes[note],0, current_z]
                moves.append(aux)
                aux2 = [x_notes[note], y_perforation, current_z]
                moves.append(aux2)
            aux3 = [0,0,current_z]
            moves.append(aux3)
        current_z += z_roller
        moves.append([0,0,current_z])

        # JAVIBG this is the moves array for the current song. moves = [[0, 0, 0], [10, 0, 0], [10, 1, 0], [0, 0, 0], [0, 0, 4], [0, 0, 8], [12, 0, 8], [12, 1, 8], [0, 0, 8], [0, 0, 12], [0, 0, 16], [14, 0, 16], [14, 1, 16], [0, 0, 16], [0, 0, 20], [0, 0, 24], [16, 0, 24], [16, 1, 24], [0, 0, 24], [0, 0, 28], [0, 0, 32], [20, 0, 32], [20, 1, 32], [0, 0, 32], [0, 0, 36], [0, 0, 40], [24, 0, 40], [24, 1, 40], [0, 0, 40], [0, 0, 44], [0, 0, 48], [28, 0, 48], [28, 1, 48], [0, 0, 48], [0, 0, 52], [0, 0, 56], [0, 0, 60], [0, 0, 64], [0, 0, 68], [10, 0, 68], [10, 1, 68], [14, 0, 68], [14, 1, 68], [20, 0, 68], [20, 1, 68], [0, 0, 68], [0, 0, 72]]

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