"""
This is the main pyRepRap module for controlling a serial
SNAP RepRap machine.

Import this module and use the internally created objects
reprap.cartesian and reprap.extruder to control the machine.



Example:

# Import the reprap  modules
import reprap, time

# Initialise serial port, here the first port (0) is used.
reprap.openSerial( 0, 19200, 1 )

# These devices are present in network, will automatically scan in the future.
reprap.cartesian.x.active = True
reprap.cartesian.y.active = True
reprap.cartesian.z.active = True
reprap.extruder.active = True

# Set axies to notify arrivals
reprap.cartesian.x.setNotify()
reprap.cartesian.y.setNotify()
reprap.cartesian.z.setNotify()

# Set stepper speed to 220 (out of 255)
reprap.cartesian.setSpeed(220)
# Set power to 83%
reprap.cartesian.setPower(83)

# The module is now ready to recieve commands #

# Send all axies to home position. Wait until arrival.
reprap.cartesian.homeReset()

# When using seek with no waitArrival = True/False argument, it defaults to true
# Seek to X1000, Y1000
reprap.cartesian.seek( (1000, 1000, None) )

# Pause
time.sleep(2)

# Seek to X500, Y1000
reprap.cartesian.seek( (500, 1000, None) )

time.sleep(2)

# Seek to X1000, Y500
reprap.cartesian.seek( (1000, 500, None) )

time.sleep(2)

# Seek to X100, Y100
reprap.cartesian.seek( (100, 100, None) )

# Send all axies to home position. Wait until arrival.
reprap.cartesian.homeReset()

# Shut off power to all motors.
reprap.cartesian.free()


"""

# Python module properties
__author__ = "Stefan Blanke (greenarrow) (greenarrow@users.sourceforge.net)"
__credits__ = ""
__license__ = "GPL 3.0"
__version__ = "2.0"
__licence__ = """
pyRepRap is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

pyRepRap is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with pyRepRap.  If not, see <http://www.gnu.org/licenses/>.
"""

# Import modules
import snap, time, serial, math, exceptions

# Enable / Diable debug
printDebug = False
#printDebug = True

# SNAP Control Commands - Taken from PIC code #
# extruder commands #
CMD_VERSION       =  0
CMD_FORWARD       =  1
CMD_REVERSE       =  2
CMD_SETPOS        =  3
CMD_GETPOS        =  4
CMD_SEEK          =  5
CMD_FREE          =  6
CMD_NOTIFY        =  7
CMD_ISEMPTY       =  8
CMD_SETHEAT       =  9
CMD_GETTEMP       = 10
CMD_SETCOOLER     = 11
CMD_PWMPERIOD     = 50
CMD_PRESCALER     = 51
CMD_SETVREF       = 52
CMD__setTempScaler = 53
CMD_GETDEBUGINFO  = 54
CMD_GETTEMPINFO   = 55
# stepper commands #
CMD_VERSION			=   0
CMD_FORWARD			=   1
CMD_REVERSE			=   2
CMD_SETPOS			=   3
CMD_GETPOS			=   4
CMD_SEEK			=   5
CMD_FREE			=   6
CMD_NOTIFY			=   7
CMD_SYNC			=   8
CMD_CALIBRATE		=   9
CMD_GETRANGE		=  10
CMD_DDA				=  11
CMD_FORWARD1		=  12
CMD_BACKWARD1		=  13
CMD_SETPOWER		=  14
CMD_GETSENSOR		=  15
CMD_HOMERESET		=  16
CMD_GETMODULETYPE	= 255
# DDA sync modes #
SYNC_NONE	= 0		# no sync (default)
SYNC_SEEK	= 1		# synchronised seeking
SYNC_INC	= 2		# inc motor on each pulse
SYNC_DEC	= 3		# dec motor on each pulse

# Motor direction modes
MOTOR_FORWARD = 1
MOTOR_BACKWARD = 2

# Unit types
UNITS_MM = 1
UNITS_STEPS = 2
UNITS_INCHES = 3	# Not implemented TODO

# Local address of host PC. This will always be 0.
snap.localAddress = 0


class _RepRapError(exceptions.Exception):
	"""RepRap Exception"""
	def __init__(self, msg):
		self.msg = msg
		return
	
	def __str__(self):
		return "A RepRap error has occured! " + str(self.msg)


def openSerial( port = 0, rate = 19200, timeout = 1 ):
	"""Open serial port for SNAP RepRap communications"""
	snap.openSerial(port = port, rate = rate, tout = timeout)


def closeSerial():
	"""Close serial port for SNAP RepRap communications"""
	snap.closeSerial()


def _bytes2int(LSB, MSB):
	"""Convert two 8 bit bytes to one integer"""
	return int( (0x100 * int(MSB) ) | int(LSB) )


def _int2bytes(val):
	"""Convert integer to two 8 bit bytes"""
	MSB = int( ( int(val) & 0xFF00) / 0x100 )
	LSB = int( int(val) & 0xFF )
	return LSB, MSB


def scanNetwork():
	"""Scan reprap network for devices (incomplete) - this will be used by autoconfig functions when complete"""
	devices = []
	for remoteAddress in range(2, 6):									# For every address in range. full range will be 255
		print "Trying address " + str(remoteAddress)
		p = snap.Packet( remoteAddress, snap.localAddress, 0, 1, [CMD_GETMODULETYPE] )	# Create snap packet requesting module type
		#p = snap.Packet( remoteAddress, snap.localAddress, 0, 1, [CMD_VERSION] )
		p.send()											# Send snap packet, if sent ok then await reply
		rep = p.getReply()
		print "dev", remoteAddress
		#devices.append( { 'address':remoteAddress, 'type':rep.dataBytes[1], 'subType':rep.dataBytes[2] } )	# If device replies then add to device list.
		time.sleep(0.5)
	for d in devices:
		#now get versions
		print "device", d


def testComms():
	"""Test that serial communications are working properly with simple loopback (incomplete)"""
	p = snap.Packet( snap.localAddress, snap.localAddress, 0, 1, [255, 0] ) 	#start sync
	p.send()
	notif = _getNotification( serialPort )
	if notif.dataBytes[0] == 255:
		return True
	return False


def _getNotification(serialPort):
	"""Return notification packet"""
	return snap.getPacket()


class extruderClass:
	tempScaler = 0
	"""Used to control a RepRap thermoplast extruder.
	An instance is automatically created in the reprap module, so for
	basic use just use reprap.extruder rather than defining a new one
	"""
	def __init__(self):
		"""Create an extruder instance. An instance is automatically create at reprap.extruder."""
		self.address = 8
		self.active = False
		self.requestedTemperature = 0
		self.vRefFactor = 7			# Default in java (middle)
		self.hb = 20				# Variable Preference
		self.hm = 1.66667			# Variable Preference
		self.absZero = 273.15		# Static, K
		#self.rz = 29000				# Variable Preference
		self.rz = 353700			# Variable Preference
		#self.beta = 3480			# Variable Preference
		self.beta = 4267			# Variable Preference
		self.vdd = 5.0				# Static, volts
		self.cap = 0.0000001		# Variable Preference
	
	def _getModuleType(self):
		"""Returns module type as an integer.
		note: do pics not support this yet? I can't see it in code and get no reply from pic"""
		p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_GETMODULETYPE] )	# Create SNAP packet requesting module type
		p.send()
		rep = p.getReply()
		rep.checkReply(2, CMD_GETMODULETYPE)
		data = rep.dataBytes
		return data[1]
	
	def getVersion(self):
		"""Returns (major, minor) firmware version as a two integer tuple."""
		p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_VERSION] )
		p.send()
		rep = p.getReply()
		rep.checkReply(3, CMD_VERSION)
		data = rep.dataBytes
		return data[1], data[2]
	
	def setMotor(self, direction, speed):
		"""Set motor direction (reprap.MOTOR_BACKWARD or reprap.MOTOR_FORWARD) and speed (0-255)"""
		if int(direction) > 0 and int(direction) < 3 and int(speed) >= 0 and int(speed <= 0xFF):
			p = snap.Packet( self.address, snap.localAddress, 0, 1, [int(direction), int(speed)] ) ##no command being sent, whats going on?
			p.send()
		else:
			raise _RepRapError("Invalid direction or speed value")
	
	def _calculateResistance(self, picTemp, calibrationPicTemp):
		"""Calculate resistance from pic timer value"""
		# TODO remove hard coded constants
		# TODO should use calibration value instead of first principles
		
		scale = 1 << (self.tempScaler+1)
		# calculate clock in hertz
		clock = 4000000.0 / (4.0 * scale)
		
		# calculate vRef in volts
		vRef = 0.25 * self.vdd + self.vdd * self.vRefFactor / 32.0
		
		# calculate time in seconds
		T = picTemp / clock
		# calc resistance in ohms
		resistance = -T / (math.log(1 - vRef / self.vdd) * self.cap)
		return resistance

	def _calculateTemperature(self, resistance):
		"""Calculate temerature from resistance"""
		return (1.0 / (1.0 / self.absZero + math.log(resistance/self.rz) / self.beta)) - self.absZero;
	
	def _rerangeTemperature(self, rawHeat):
		"""Adjust temperature range"""
		notDone = False
		if (rawHeat == 255 and self.vRefFactor > 0):
			self.vRefFactor -= 1
			# extruder re-ranging temperature (faster)
			self._setTempRange()
		elif (rawHeat < 64 and self.vRefFactor < 15):
			self.vRefFactor += 1
			# extruder re-ranging temperature (slower)
			self._setTempRange()
		else:
			notDone = True
		return notDone
	
	def _setTempRange(self):
		"""Set tempertaure range"""
		# We will send the vRefFactor to the PIC.  At the same
		# time we will send a suitable temperature scale as well.
		# To maximize the range, when vRefFactor is high (15) then
		# the scale is minimum (0).
		
		#extruder vRefFactor set to " + vRefFactor
		self.tempScaler = 7 - (self.vRefFactor >> 1);
		self._setVoltageReference(self.vRefFactor)
		self._setTempScaler(self.tempScaler)
		if (self.requestedTemperature != 0):
			self.setTemp(self.requestedTemperature, False)
			# should we be re-calling this function to make sure temp request is updated (rather than just calling it once. are we?)
	
	def _getRawTemp(self):
		"""Get raw temperature pic timer value"""
		p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_GETTEMP] )
		p.send()
		rep = p.getReply()
		rep.checkReply(3, CMD_GETTEMP)
		data = rep.dataBytes
		#rawTemp, calibration
		return data[1], data[2]
	
	def getTemp(self):
		"""Returns current extruder temperature in degrees Celsius as an integer"""
		self._autoTempRange()
		rawHeat, calibration = self._getRawTemp()
		while rawHeat == 255 or rawHeat == 0:
			# TODO very bad as will hang in what should be a tight loop
			time.sleep(0.1)
			self._autoTempRange()
		res = self._calculateResistance( rawHeat, calibration )
		temp = self._calculateTemperature(res)
		#print rawHeat, res, temp
		return round(temp, 1)	#there is no point returning more points than this
		
	def setTemp(self, temperature, null):
		"""Set the extruder target temperature (degrees Celsius)"""
		self.requestedTemperature = temperature
		#if(math.abs(self.requestedTemperature - extrusionTemp) > 5):
		#	print " extruder temperature set to " + self.requestedTemperature + "C, which is not the standard temperature (" + extrusionTemp + "C).")
		# Aim for 10% above our target to ensure we reach it.  It doesn't matter
		# if we go over because the power will be adjusted when we get there.  At
		# the same time, if we aim too high, we'll overshoot a bit before we
		# can react.
		
		# Tighter temp constraints under test 10% -> 3% (10-1-8)
		temperature0 = temperature * 1.03
		
		# A safety cutoff will be set at 20% above requested setting
		# Tighter temp constraints added by eD 20% -> 6% (10-1-8)
		temperatureSafety = temperature * 1.06
		
		# Calculate power output from hm, hb.  In general, the temperature
		# we achieve is power * hm + hb.  So to achieve a given temperature
		# we need a power of (temperature - hb) / hm
		
		# If we reach our temperature, rather than switching completely off
		# go to a reduced power level.
		power0 = int( round(((0.9 * temperature0) - self.hb) / self.hm) )
		if power0 < 0: power0 = 0
		if power0 > 255: power0 = 255

		# Otherwise, this is the normal power level we will maintain
		power1 = int( round((temperature0 - self.hb) / self.hm) )
		if power1 < 0: power1 = 0
		if power1 > 255: power1 = 255

		# Now convert temperatures to equivalent raw PIC temperature resistance value
		# Here we use the original specified temperature, not the slight overshoot
		resistance0 = self._calculateResistanceForTemperature(temperature)
		resistanceSafety = self._calculateResistanceForTemperature(temperatureSafety)

		# Determine equivalent raw value
		t0 = self._calculatePicTempForResistance(resistance0)
		if t0 < 0: t0 = 0
		if t0 > 255: t0 = 255
		t1 = self._calculatePicTempForResistance(resistanceSafety)
		if t1 < 0: t1 = 0
		if t1 > 255: t1 = 255
		
		if (temperature == 0):
			self._setHeat( 0, 0, 0, 0 )
		else:
			self._setHeat( power0, power1, t0, t1 )

	def _calculateResistanceForTemperature(self, temperature):
		return self.rz * math.exp(self.beta * (1/(temperature + self.absZero) - 1/self.absZero))

	def _calculatePicTempForResistance(self, resistance):
		scale = 1 << (self.tempScaler+1)
		clock = 4000000.0 / (4.0 * scale)	#// hertz		
		vRef = 0.25 * self.vdd + self.vdd * self.vRefFactor / 32.0	#// volts
		T = -resistance * (math.log(1 - vRef / self.vdd) * self.cap)
		picTemp = T * clock
		return int( round(picTemp) )
	
	def _autoTempRange(self):
		"""Automatically find the suitable temperature range"""
		rawHeat, calibration = self._getRawTemp()
		unChanged = self._rerangeTemperature(rawHeat)
		while not unChanged:
			rawHeat, calibration = self._getRawTemp()
			unChanged = self._rerangeTemperature(rawHeat)
			time.sleep(0.1)
	
	def _setVoltageReference(self, val):
		p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_SETVREF, int(val)] )
		p.send()
	
	def _setTempScaler(self, val):
		p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD__setTempScaler, int(val)] )
		p.send()
	
	def _setHeat(self, lowHeat, highHeat, tempTarget, tempMax):
		print "Setting heater with params, ", "lowHeat", lowHeat, "highHeat", highHeat, "tempTarget", tempTarget, "tempMax", tempMax
		tempTargetMSB, tempTargetLSB = _int2bytes( tempTarget )
		tempMaxMSB ,tempMaxLSB = _int2bytes( tempMax )
		p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_SETHEAT, int(lowHeat), int(highHeat), tempTargetMSB, tempTargetLSB, tempMaxMSB, tempMaxLSB] )	# assumes MSB first (don't know this!)
		p.send()
	
	def setCooler(self, speed):
		"""Set the speed (0-255) of the cooling fan"""
		p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_SETCOOLER, int(speed)] )
		p.send()
	
	def freeMotor(self):
		"""Power off the extruder motor"""
		p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_FREE] )
		p.send()

extruder = extruderClass()



class axisClass:
	"""Used to control a RepRap axis.
	X, Y & Z instances are automatically created in the reprap module,
	so for basic use just use reprap.cartesian.x, reprap.cartesian.y
	& reprap.cartesian.z  rather than defining new ones.
	
	Most of the time the functions in reprap.cartesian will be 
	sufficient. E.g. to move just one axis you can use:
	reprap.cartesian.seek((200, None, None))
	"""
	def __init__(self, address):
		"""Create an axis instance with address (1-255). Instances for the three axies are automatically created and can be accessed via reprap.cartesian.x, reprap.cartesian.y & reprap.cartesian.z
		These instances are pre-configured with the correct addresses and alow the use of whole machine reprap.cartesian commands."""
		self.address = address
		# when scanning network, set this, then in each func below, check alive before doing anything TODO
		self.active = False
		self.limit = 0
		self.speed = None
		self.stepsPerMM = None
		self.units = None
	
	def forward1(self):
		"""Move axis one step forward
		Return the completed state as a bool."""
		p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_FORWARD1] ) 
		p.send()
	
	def backward1(self):
		"""Move axis one step backward"""
		p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_BACKWARD1] ) 
		p.send()
	
	def forward(self, speed = None):
		"""Spin axis forward at given speed (0-255)
		If no speed is specified then a value must have been previously set with axisClass.setSpeed()"""
		if speed == None:
			speed = self.speed
			if speed == None:
				raise _RepRapError("Axis speed not set")
		if speed >=0 and speed <= 0xFF:
			p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_FORWARD, int(speed)] ) 
			p.send()
		else:
			raise _RepRapError("Invalid speed value")
	
	def backward(self, speed = None):
		"""Spin axis backward at given speed (0-255)
		If no speed is specified then a value must have been previously set with axisClass.setSpeed()"""
		# If speed is not specified use stored (or default)
		if speed == None:
			speed = self.speed
			if speed == None:
				raise _RepRapError("Axis speed not set")
		if speed >=0 and speed <= 0xFF:
			p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_REVERSE, int(speed)] ) 
			p.send()
		else:
			raise _RepRapError("Invalid speed value")
	
	def getSensors(self):
		"""Debug only. Returns raw PIC port bytes)"""
		p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_GETSENSOR] )
		p.send()
		rep = p.getReply()
		rep.checkReply(3, CMD_GETSENSOR)
		data = rep.dataBytes
		print data[1], data[2]
	
	def getPos(self):
		"""Return the axis postition as an integer (steps)."""
		p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_GETPOS] )
		p.send()
		rep = p.getReply()
		rep.checkReply(3, CMD_GETPOS)
		data = rep.dataBytes
		pos = _bytes2int( data[1], data[2] )
		return pos
	
	def setPos(self, pos):
		"""set current position (integer) (set variable, not physical position. units as steps)"""
		if pos >=0 and pos <= 0xFFFF:
			posMSB ,posLSB = _int2bytes( pos )
			p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_SETPOS, posMSB, posLSB] )
			p.send()
		else:
			raise _RepRapError("Invalid position value")
	
	def free(self):
		"""Power off coils on stepper"""
		p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_FREE] ) 
		p.send()
	
	def seek(self, pos, speed = None, waitArrival = True, units = None):
		"""Seek to axis location pos. If waitArrival is True, funtion waits until seek is compete to return"""
		
		# If no speed is specified use value set for axis, if this doesn't exist raise exception
		if speed == None:
			speed = self.speed
			if speed == None:
				raise _RepRapError("Axis speed not set")
		
		# If no units area specified use units set for axis, if this doesn't exist use steps
		if units == None:
			units = self.units
			if units == None:
				units = UNITS_STEPS
		
		# Convert units into steps
		if units == UNITS_STEPS:
			pass
		elif units == UNITS_MM:
			newX = int( float(newX) * self.stepsPerMM )
			newY = int( float(newY) * self.stepsPerMM )
		elif units == UNITS_INCHES:
			newX = int( float(newX) * self.stepsPerMM * 25.4 )
			newX = int( float(newY) * self.stepsPerMM * 25.4 )
		else:
			raise _RepRapError("Invalid units")
		
		# Check that position is withing limits and speed is valid
		if (pos <= self.limit or self.limit == 0) and pos >=0 and pos <= 0xFFFF and speed >=0 and speed <= 0xFF:
			posMSB ,posLSB = _int2bytes( pos )
			p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_SEEK, int(speed), posMSB ,posLSB] ) 
			p.send()
			if waitArrival:
				notif = p.getReply()
				if not notif.dataBytes[0] == CMD_SEEK:
					raise _RepRapError("Expected seek notification")
		else:
			raise _RepRapError("Invalid speed or position value")
	
	def homeReset(self, speed = None, waitArrival = True):
		"""Go to 0 position. If waitArrival is True, funtion waits until reset is compete to return"""
		if speed == None:
			speed = self.speed
			if speed == None:
				raise _RepRapError("Axis speed not set")
		if speed >= 0 and speed <= 0xFF:
			p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_HOMERESET, int(speed)] ) 
			p.send()
			if waitArrival:
				notif = p.getReply()
				if not notif.dataBytes[0] == CMD_HOMERESET:
					raise _RepRapError("Expected home reset notification")
		else:
			raise _RepRapError("Invalid speed value")
	
	def setNotify(self):
		"""Set axis to notify on arrivals"""
		p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_NOTIFY, snap.localAddress] ) 	# set notifications to be sent to host
		p.send()
	
	def _setSync( self, syncMode ):
		"""Set sync mode"""
		if syncMode >= 0 and syncMode <= 0xFF:
			p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_SYNC, int(syncMode)] )
			p.send()
		else:
			raise _RepRapError("Invalid sync mode")
	
	def _DDA( self, seekTo, slaveDelta, speed = False, waitArrival = True):
		"""Set DDA mode"""
		if not speed:
			speed = self.speed
		if (seekTo <= self.limit or self.limit == 0) and seekTo >=0 and seekTo <= 0xFFFF and slaveDelta >=0 and slaveDelta <= 0xFFFF and speed >=0 and speed <= 0xFF:
			masterPosMSB, masterPosLSB = _int2bytes( seekTo )
			slaveDeltaMSB, slaveDeltaLSB = _int2bytes( slaveDelta )
			p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_DDA, int(speed), masterPosMSB ,masterPosLSB, slaveDeltaMSB, slaveDeltaLSB] ) 	#start sync
			p.send()
			if waitArrival:
				notif = p.getReply()
				if not notif.dataBytes[0] == CMD_DDA:
					raise _RepRapError("Expected DDA notification")
	
	def setPower( self, power ):
		"""Set stepper motor power (0-100%)"""
		power = int( float(power) * 0.63 )
		if power >=0 and power <=0x3F:
			#p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_SETPOWER, int( power * 0.63 )] ) # This is a value from 0 to 63 (6 bits)
			p = snap.Packet( self.address, snap.localAddress, 0, 1, [CMD_SETPOWER, power] ) # This is a value from 0 to 63 (6 bits)
			p.send()
		else:
			raise _RepRapError("Invalid power value")
	
	def setSpeed(self, speed):
		"""Set axis move speed (0-255)"""
		if speed >= 0 and speed <= 0xFF:
			self.speed = speed
		else:
			raise _RepRapError("Invalid speed value")
	
	def setStepsPerMM(self, spmm):
		"""Set axis steps per millimeter"""
		self.stepsPerMM = float(spmm)
	
	def setUnits(self, units):
		"""Set axis units (reprap.UNITS_STEPS/reprap.UNITS_MM/reprap.UNITS_INCHES)"""
		if self.stepsPerMM == None:
			raise _RepRapError("Only reprap.UNITS_STEPS can be used without specifying steps per mm. use setStepsPerMM().")
		else:
			self.units = units


class _syncAxis:
	"""Class to hold axis info so master and slave on sync move can be swapped over."""
	def __init__( self, axis, seekTo, delta, direction ):
		self.axis = axis
		self.seekTo = seekTo
		self.delta = delta
		self.direction = direction
		
		if self.direction > 0:
			self.syncMode = SYNC_INC
		else:
			self.syncMode = SYNC_DEC

class cartesianClass:
	"""Main cartesian robot class"""
	def __init__(self):
		"""Create a cartesian instance. An instances is automatically created and can be accessed via reprap.cartesian
		You will only need to create another one in you want to control more than one machine at the same time."""
		
		self.units = None
		
		# initiate axies with addresses
		self.x = axisClass(2)
		self.y = axisClass(3)
		self.z = axisClass(4)
	
	def homeReset(self, speed = None, waitArrival = True):
		"""Reset all axies to the home position with speed (0-255) and wait until arrival (boolean)
		WARNING : Reseting all axies at the same time is unstable. suggest using waitArrival = True option."""
		if not waitArrival:
			print "WARNING : Reseting all axies at the same time is unstable. suggest using waitArrival = True option."
		#z is done first so we don't break anything we just made
		if self.z.active:
			if self.z.homeReset( speed = speed, waitArrival = waitArrival ):
				print "Z Reset"
		
		if self.x.active:
			if self.x.homeReset( speed = speed, waitArrival = waitArrival ):
				print "X Reset"
		
		if self.y.active:
			if self.y.homeReset( speed = speed, waitArrival = waitArrival ):
				print "Y Reset"
		
		# TODO add a way to collect all three notifications (in whatever order) then check they are all there. this will allow symultanious axis movement and use of waitArrival
		# This requires snap receiver to search packets rather than just returning oldest one. probably need to do this anyway
	
	def seek(self, pos, speed = None, waitArrival = True, units = None):
		"""Seek to a position ( tuple (x, y, z) ) with speed (0-255) and and wait until arrival (boolean)
		Seek will automatically select between a standard seek and a syncronised seek when it is required
		When waitArrival is True, funtion does not return until all seeks are compete"""
		
		if speed == None:
			speed = self.speed
			if speed == None:
				raise _RepRapError("Cartesian speed not set or specified")
		
		if units == None:
			units = self.units
			if units == None:
				units = UNITS_STEPS
		
		curX, curY, curZ = self.x.getPos(), self.y.getPos(), self.z.getPos()
		x, y, z = pos
		#print "seek from", curX, curY, curZ, "to", x, y, z
		# Check that we are moving withing limits, and 
		if (x == None or x <= self.x.limit or self.x.limit == 0) and (y == None or y <= self.y.limit or self.y.limit == 0) and (z == None or z <= self.z.limit or self.z.limit == 0):
			if printDebug: print "seek from [", curX, curY, curZ, "] to [", x, y, z, "]"
			# Check if we need to do a standard seek or a DDA seek
			if x == curX or y == curY or x == None or y == None:
				if printDebug: print "    standard seek"
				if x != None and x != curX:				
					self.x.seek( x, speed, waitArrival, units )			#setting these to true breaks waitArrival convention. need to rework waitArrival and possibly have each axis storing it's arrival flag and pos as variables?
				if y != None and y != curY:
					self.y.seek( y, speed, waitArrival, units )
			elif x != None and y != None:
				if printDebug: print "    sync seek"
				self._syncSeek( pos, curX, curY, speed, waitArrival, units )
			if z != None and z != curZ:
				#self.z.seek( z, speed, True ) # why was this forced?
				self.z.seek( z, speed, waitArrival, units )
			return True
		else:
			print "Trying to print outside of limit, aborting seek"
			return False
	
	def _syncSeek(self, pos, curX, curY, speed, waitArrival, units):
		"""perform syncronised x/y movement. This is called by seek when needed."""
		newX, newY, nullZ = pos
		if units == UNITS_STEPS:
			pass
		elif units == UNITS_MM:
			newX = int( float(newX) * self.stepsPerMM )
			newY = int( float(newY) * self.stepsPerMM )
		elif units == UNITS_INCHES:
			newX = int( float(newX) * self.stepsPerMM * 25.4 )
			newX = int( float(newY) * self.stepsPerMM * 25.4 )
		else:
			raise _RepRapError("Invalid units")
		
		deltaX = abs( curX - newX )		# calc delta movements
		deltaY = abs( curY - newY )
		#print "syncseek deltas", deltaX, deltaY
		directionX = ( curX - newX ) / -deltaX	# gives direction -1 or 1
		directionY = ( curY - newY ) / -deltaY	
		
		master = _syncAxis( self.x, newX, deltaX, directionX )	# create two swapable data structures, set x as master, y as slave
		slave = _syncAxis( self.y, newY, deltaY, directionY )
		
		if slave.delta > master.delta:		# if y has the greater movement then make y master
			slave, master = master, slave
			if printDebug: print "    switching to y master"
		if printDebug: print "    masterPos", master.seekTo, "slaveDelta", slave.delta
		slave.axis._setSync( slave.syncMode )
		master.axis._DDA( master.seekTo, slave.delta, speed, True )	#why was this forced? # because we have to wait before we tell the slave axis to leave sync mode!
		#master.axis._DDA( master.seekTo, slave.delta, speed, waitArrival )
		time.sleep(0.01)	# TODO this is really bad, can we remove?
		slave.axis._setSync( SYNC_NONE )
		if printDebug: print "    sync seek complete"
	
	def getPos(self):
		"""Return the current positions of all three axies as a tuple (x, y, z) in steps"""
		return self.x.getPos(), self.y.getPos(), self.z.getPos()
	
	def stop(self):
		"""Stop all motors (but retain current)"""
		if self.x.active:
			self.x.forward(0)
		
		if self.y.active:
			self.y.forward(0)
		
		if self.z.active:
			self.z.forward(0)
	
	def free(self):
		"""Free all motors (no current on coils)"""
		if self.x.active:
			self.x.free()
		
		if self.y.active:
			self.y.free()
		
		if self.z.active:
			self.z.free()
	
	def setPower(self, power):
		"""Set stepper power (0-100)"""
		if self.x.active:
			self.x.setPower(power)
		
		if self.y.active:
			self.y.setPower(power)
		
		if self.z.active:
			self.z.setPower(power)
	
	#def lockout():
	#keep sending power down commands to all board every second
	
	def setSpeed(self, speed):
		"""Set axies move speed (0-255)"""
		self.speed = speed
		if self.x.active:
			self.x.setSpeed(speed)
		
		if self.y.active:
			self.y.setSpeed(speed)
		
		if self.z.active:
			self.z.setSpeed(speed)
	
	def setStepsPerMM(self, spmm):
		"""Set axies steps per millimeter"""
		self.stepsPerMM = float(spmm)
		self.x.stepsPerMM(spmm)
		self.z.stepsPerMM(spmm)
		self.z.stepsPerMM(spmm)
	
	def setUnits(self, units):
		"""Set axies units (reprap.UNITS_STEPS/reprap.UNITS_MM/reprap.UNITS_INCHES)"""
		self.units = units
		self.x.setUnits(units)
		self.y.setUnits(units)
		self.z.setUnits(units)


cartesian = cartesianClass()