################################################################################################
#			  				  				                                                   #
#     G code interpreter and executer for 2D CNC laser engraver using Raspberry Pi             #
#       Xiang Zhai,   Oct 1, 2013                         				                       #
#       zxzhaixiang at gmail.com							                                   #
#       Improved and updated for 3D support by dionys rosario                                  #
#       dionyself at gmail.com                                                                 #
#											                                                   #
################################################################################################

import RPi.GPIO as GPIO
import motor_control
from motor import Motor
import time
from numpy import pi, sin, cos, sqrt, arccos, arcsin

filename = 'Xiang.nc'  # file name of the G code commands
GPIO.setmode(GPIO.BOARD)
Laser_switch = 15
GPIO.setup(Laser_switch, GPIO.OUT)
GPIO.output(Laser_switch, False)


def XYposition(lines):
    # given a movement command line, return the X Y position
    xchar_loc = lines.index('X')
    i = xchar_loc+1
    while (47 < ord(lines[i]) < 58) | (lines[i] == '.') | (lines[i] == '-'):
        i += 1
    x_pos = float(lines[xchar_loc+1:i])

    ychar_loc = lines.index('Y')
    i = ychar_loc+1
    while (47 < ord(lines[i]) < 58) | (lines[i] == '.') | (lines[i] == '-'):
        i += 1
    y_pos = float(lines[ychar_loc + 1:i])

    return x_pos, y_pos


def IJposition(lines):
    # given a G02 or G03 movement command line, return the I J position
    ichar_loc = lines.index('I')
    i = ichar_loc + 1
    while (47 < ord(lines[i]) < 58) | (lines[i] == '.') | (lines[i] == '-'):
        i += 1
    i_pos = float(lines[ichar_loc+1:i])

    jchar_loc = lines.index('J')
    i = jchar_loc+1
    while (47 < ord(lines[i]) < 58) | (lines[i] == '.') | (lines[i] == '-'):
        i+=1
    j_pos = float(lines[jchar_loc+1:i])

    return i_pos, j_pos


def moveto(MX, x_pos, dx, MY, y_pos, dy, speed, engraving):
    """Move to (x_pos,y_pos) (in real unit)"""
    stepx = int(round(x_pos/dx))-MX.position
    stepy = int(round(y_pos/dy))-MY.position

    Total_step = sqrt((stepx**2+stepy**2))

    if Total_step > 0:
        if lines[0:3] == 'G0 ':  # fast movement
            print 'No Laser, fast movement: Dx=', stepx, '  Dy=', stepy
            Motor_control.Motor_Step(MX, stepx, MY, stepy, 50)
        else:
            print 'Laser on, movement: Dx=', stepx, '  Dy=', stepy
            Motor_control.Motor_Step(MX, stepx, MY, stepy, speed)
    return 0


def print_gcode_file(filename, **config):

    global Laser_switch
    MX = Motor(23, 22, 24, 26)     # pin number for a1,a2,b1,b2.  a1 and a2 form coil A b1 and b2 form coil B
    MY = Motor(11, 7, 5, 3)
    MZ = Motor(11, 7, 5, 3)        # experimental 3d support by Diony
    dx = congig.get('dx', 0.075)   # resolution in x direction. Unit: mm
    dy = congig.get('dy', 0.075)   # resolution in y direction. Unit: mm
    Engraving_speed = congig.get('speed', 0.1)  # unit=mm/sec=0.04in/sec
    speed=Engraving_speed/min(dx,dy)      #step/sec

    try:#read and execute G code
        with open(filename,'r') as gcode_file:
            ## validategcode(filename, gcode_file)
            for lines in gcode_file:
                if lines==[]:
                   1 #blank lines
                elif lines[0:3]=='G90':
                   print 'start'
                elif lines[0:3]=='G20':# working in inch
                    dx/=25.4
                    dy/=25.4
                    print 'Working in inch'
                elif lines[0:3]=='G21':# working in mm
                    print 'Working in mm'
                elif lines[0:3]=='M05':
                    GPIO.output(Laser_switch,False)
                    print 'Laser turned off'
                elif lines[0:3]=='M03':
                    GPIO.output(Laser_switch,True)
                    print 'Laser turned on'
                elif lines[0:3]=='M02':
                    GPIO.output(Laser_switch,False)
                    print 'finished. shuting down'
                    break
                elif (lines[0:3]=='G1F')|(lines[0:4]=='G1 F'):
                    1#do nothing
                elif (lines[0:3]=='G0 ')|(lines[0:3]=='G1 ')|(lines[0:3]=='G01'): #|(lines[0:3]=='G02')|(lines[0:3]=='G03'):
                    #linear engraving movement
                    if (lines[0:3]=='G0 '):
                        engraving=False
                    else:
                        engraving=True
                    [x_pos,y_pos]=XYposition(lines)
                    moveto(MX,x_pos,dx,MY,y_pos,dy,speed,engraving)
                elif (lines[0:3]=='G02')|(lines[0:3]=='G03'): #circular interpolation
                    old_x_pos=x_pos
                    old_y_pos=y_pos
                    [x_pos,y_pos]=XYposition(lines)
                    [i_pos,j_pos]=IJposition(lines)
                    xcenter=old_x_pos+i_pos   #center of the circle for interpolation
                    ycenter=old_y_pos+j_pos
                    Dx=x_pos-xcenter
                    Dy=y_pos-ycenter      #vector [Dx,Dy] points from the circle center to the new position
                    r=sqrt(i_pos**2+j_pos**2)   # radius of the circle
                    e1=[-i_pos,-j_pos] #pointing from center to current position
                    if (lines[0:3]=='G02'): #clockwise
                        e2=[e1[1],-e1[0]]      #perpendicular to e1. e2 and e1 forms x-y system (clockwise)
                    else:                   #counterclockwise
                        e2=[-e1[1],e1[0]]      #perpendicular to e1. e1 and e2 forms x-y system (counterclockwise)

                    #[Dx,Dy]=e1*cos(theta)+e2*sin(theta), theta is the open angle

                    costheta=(Dx*e1[0]+Dy*e1[1])/r**2
                    sintheta=(Dx*e2[0]+Dy*e2[1])/r**2        #theta is the angule spanned by the circular interpolation curve

                    if costheta>1:  # there will always be some numerical errors! Make sure abs(costheta)<=1
    		            costheta=1
    	            elif costheta<-1:
    		            costheta=-1
                    theta=arccos(costheta)
                    if sintheta<0:
                        theta=2.0*pi-theta

                    no_step=int(round(r*theta/dx/5.0))   # number of point for the circular interpolation

                    for i in range(1,no_step+1):
                        tmp_theta=i*theta/no_step
                        tmp_x_pos=xcenter+e1[0]*cos(tmp_theta)+e2[0]*sin(tmp_theta)
                        tmp_y_pos=ycenter+e1[1]*cos(tmp_theta)+e2[1]*sin(tmp_theta)
                        moveto(MX,tmp_x_pos,dx,MY, tmp_y_pos,dy,speed,True)

    except KeyboardInterrupt:
        pass
    finally:
        GPIO.output(Laser_switch,False)   # turn off laser
        moveto(MX,0,dx,MY,0,dy,50,False)  # move back to Origin
        MX.unhold()
        MY.unhold()
        GPIO.cleanup()
