import serial
import math
from time import sleep

XMIN=0
XMAX = 44000
YMIN  = 0
YMAX = 44000

currentX = 0
currentY = 0

print "Opening Serial Port!"
sp = serial.Serial("/dev/ttyUSB0",9600, timeout=0.25, rtscts=0)

print "sp", sp



#Moves to a specified X/Y location (in stepper coordinates)
def stepperMoveTo(x,y):
   global currentX
   global currentY

   if(x < XMIN or y < YMIN) or (x > XMAX or y > YMAX):
      print "ERROR!  coordinates out of range!", x, y
      quit()

   cmdString = "@01 amov %d %d n n\r" % ( x, y)
   sp.write(cmdString)


   #Wait until we are done.
   line = sp.readline()
   while( not '!' in line):
	line = sp.readline()

   currentX = x
   currentY = y
   return 


#Interpelate the points between current location and end point, moving
#stepSize at a time.
def lineTo(x,y,stepSize):
	global currentX
	global currentY

	runX = x - currentX + 0.0
  	runY = y - currentY + 0.0

	slope = runY / runX
	yIntercept = currentY - (slope * currentX)



	if (runX < 0):
		xInc = -stepSize
	else:
		xInc = stepSize


	while(abs(currentX - x) > stepSize*1.5):	
	   nowX = currentX + xInc 
	   nowY = slope * nowX + yIntercept 
	   stepperMoveTo(nowX,nowY)  #modifies currentX/Y


	#Final move
	stepperMoveTo(x,y)



def cPoint(tick,range,radius):
   angle = tick * (360.0 / range)
   radiansPerDegree = math.pi / 180.0
   x = int( round( radius * math.sin(angle * radiansPerDegree) ) )
   y = int( round( radius * math.cos(angle * radiansPerDegree)))
   return( (x,y) )

def circle(numPoints, radius, centerX,centerY):
   points = []
   for i in range(numPoints):
      x,y = cPoint(i+1, numPoints, radius-4)
      scaledX, scaledY = (x+centerX), (centerY - y)
      points.append( (scaledX, scaledY) )
   return points

   


#points =  circle(361, 21500, 21500,21500) 
#points.reverse()
#print points

# Point 1
stepperMoveTo(11500,29650)
raw_input("Drop pen, then press enter!")

#point 2
stepperMoveTo(7500,29650)

#point 3
stepperMoveTo(7500,17800)

#point 4
stepperMoveTo(5700,17800)

#point 5
stepperMoveTo(5700,10000)

#point 6
lineTo(8200,8000,50)

#point 7
stepperMoveTo(8200,5600)


#point 8
lineTo(15200,1700,50)

#point 9
lineTo(17900,6850,50)

#point 10
lineTo(12000,9900,50)

#point 11
lineTo(11500,11500,50)

#Back to point 1
stepperMoveTo(11500,29650)



print "Lift pen!"
sp.close()
quit()