; Rectangular pocket - clears using spiral then corner-clearing helper sub
; Params: #1=x1, #2=y1, #3=x2, #4=y2, #5=zstart, #6=zend, #7=fincut, #8=mode
; Mode: 0=CCW/ramp, 1=CW/ramp, 2=CCW/plunge, 3=CW/plunge

; pre-declare named params used across sub/helper scope
#<a> = 0
#<z> = 0
#<td> = 0
#<z_clearance> = 0
#<rampang> = 0
#<z_down>  = 0
#<z_lift>  = 0
#<minx> = 0
#<maxx> = 0
#<cx> = 0
#<cy> = 0
#<rx> = 0
#<ry> = 0

o<pocket_rect> sub
	; x1, y1, x2, y2, zstart, zend, fincut, mode
	; #1  #2  #3  #4   #5      #6     #7     #8
	; modes:
	;  0: CCW, ramped plunge
	;  1: CW,  ramped plunge
	;  2: CCW, straight plunge
	;  3: CW,  straight plunge

	G90	  ; absolute x,y,z
	G90.1 ; absolute i,j,k
	M101  ; enable Z-axis (M102 disables)
	G17   ; select XY plane for arcs

	#<td> = #<_td>
	o1 if [EXISTS[#<_z_clearance>]]
		#<z_clearance> = #<_z_clearance>
	o1 else
		#<z_clearance> = #5
	o1 endif
	o2 if [EXISTS[#<_rampang>]]
		#<rampang> = #<_rampang>
	o2 else
		#<rampang> = 5 ; degrees
	o2 endif
	o3 if [EXISTS[#<_stepover>]]
		#<stepover> = #<_stepover>
	o3 else
		#<stepover> = [#<td>*0.4]
	o3 endif

	#<z_down> = #6
	#<z_lift> = [#6+#<td>*0.1] ; slight lift for repositioning moves

	#<minx> = #1
	#<maxx> = #3
	o11 if [#3 LT #1]
		#<minx> = #3
		#<maxx> = #1
	o11 endif

	#<miny> = #2
	#<maxy> = #4
	o12 if [#4 LT #2]
		#<miny> = #4
		#<maxy> = #2
	o12 endif

	#<cx> = [[#<minx>+#<maxx>]/2]  ; pocket center X
	#<cy> = [[#<miny>+#<maxy>]/2]  ; pocket center Y
	#<rx> = [[[#<maxx>-#<minx>]-#<td>]/2] ; half-width minus tool radius
	#<ry> = [[[#<maxy>-#<miny>]-#<td>]/2] ; half-height minus tool radius

	G0 X#<cx> Y#<cy>
	G1 X[#<cx>+#<rx>] Y[#<cy>-#<ry>]
	G1 X[#<cx>-#<rx>] Y[#<cy>-#<ry>]
	G1 X[#<cx>-#<rx>] Y[#<cy>+#<ry>]
	G1 X[#<cx>+#<rx>] Y[#<cy>+#<ry>]

	o100 if [[#8 EQ 0] OR [#8 EQ 1]] ; helical plunge
		#<r> = [#<td>*0.3]
		#<z_step>  = [#<r>*2*3.14*TAN[#<rampang>]]

		G0 Z#<z_clearance>
		G0 X[#<cx>+#<r>] Y[#<cy>]
		G0 Z#5

		; helical plunge
		#<z> = [#5-#<z_step>]
		o101 while [#<z> GT #6]
			o102 if [#8 EQ 0]
				G3 X[#<cx>+#<r>] Y[#<cy>] I[#<cx>] J[#<cy>] Z[#<z>] P1
			o102 else
				G2 X[#<cx>+#<r>] Y[#<cy>] I[#<cx>] J[#<cy>] Z[#<z>] P1
			o102 endif
			#<z> = [#<z> - #<z_step>]
		o101 endwhile

		; finish the helical plunge
		#<a> = [[#<z>-#6]/#5*360]
		o103 if [#8 EQ 0]
			G3 X[#<cx>+COS[#<a>]*#<r>] Y[#<cy>+SIN[#<a>]*#<r>] I[#<cx>] J[#<cy>] Z[#6]
			G3 X[#<cx>+COS[#<a>]*#<r>] Y[#<cy>+SIN[#<a>]*#<r>] I[#<cx>] J[#<cy>] Z[#6]
		o103 else
			G2 X[#<cx>+COS[#<a>]*#<r>] Y[#<cy>-SIN[#<a>]*#<r>] I[#<cx>] J[#<cy>] Z[#6]
			G2 X[#<cx>+COS[#<a>]*#<r>] Y[#<cy>-SIN[#<a>]*#<r>] I[#<cx>] J[#<cy>] Z[#6]
		o103 endif

	o100 elseif [[#8 EQ 2] OR [#8 EQ 3]] ; straight plunge
		#<r> = 0

		G0 Z#5
		G0 X[#<cx>] Y[#<cy>]
		G1 Z[#6]

	o100 endif

	; spiral outward from center until hitting pocket boundary
	#<r_base> = #<r>
	#<a_base> = #<a>
	o105 while [1]

		o106 if [[#8 MOD 2] EQ 0]
			G1 X[#<cx>+COS[#<a>]*#<r>] Y[#<cy>+SIN[#<a>]*#<r>]
		o106 else
			G1 X[#<cx>+COS[#<a>]*#<r>] Y[#<cy>-SIN[#<a>]*#<r>]
		o106 endif
		
		#<a> = [#<a>+1] ; next angle
		#<r> = [#<r_base>+[#<a>-#<a_base>]/360*#<stepover>] ; compute the radius accordingly


		o110 if [ABS[COS[#<a>]*#<r>] GE [#<rx>-#7]]
			; hit the x limit, y is still 'unbounded'
			o105 BREAK
		o110 elseif [ABS[SIN[#<a>]*#<r>] GE [#<ry>-#7]]
			; hit the y limit, x is still 'unbounded'
			o105 BREAK
		o110 endif
	o105 endwhile

	; okay, let's run until we hit a limit
	; then based on which limit was hit, run in the perp. direction
	; then go into the negative of that direction
	; subroutines might be the best way to do this??

;	o200 while [#<r> LE 3]
;		#<a> = [#<a>+1] ; next angle
;
;
;		o201 if [[[COS[#<a>]*#<r> GE -#<rx>] AND [COS[#<a>]*#<r> LE #<rx>]] AND [[SIN[#<a>]*#<r> GE -#<ry>] AND [SIN[#<a>]*#<r> LE #<ry>]]]
;			G1 X[#<cx>+COS[#<a>]*#<r>] Y[#<cy>+SIN[#<a>]*#<r>]
;		o201 endif
;
;		#<a> = [#<a>+0.5] ; next angle
;		#<r> = [#<r_base>+[#<a>-#<a_base>]/360*#<stepover>] ; compute the radius accordingly
;	o200 endwhile

	

	o<xp> call ; clear corners that the spiral couldn't reach



	G0 Z#<z_clearance>


o<pocket_rect> endsub

; if you can come up with a closed-form solution for x*sin(x) = c, you could drastically streamline this code. Until then... Lots of g1s.
; idea: bias the main spiral. then you're left with a section to hog out
; idea 2: make angle arbitrary (blowing up the code so it's not x-y specific). That'll be fun.

; Helper sub: clears rectangular corner material left by circular spiral
; Walks along the X-axis edge, tracing arcs that match the spiral boundary
o<xp> sub
		#<ix> = 0
		#<lastxx> = 0
		#<run> = 1
		o202 while [#<run>]
			G0 Z#<z_lift>
			G0 X#<cx> Y[#<cy> - #<ry>]
			G1 Z#<z_down>

			#<n_end> = FUP[sqrt[[#<ix>]**2 + [#<ry>]**2]/#<stepover>] ; spiral revolution number at this corner point
			#<a> = ATAN[-#<ry>]/[#<ix>]
			#<r_end> = [#<n_end>*#<stepover> + #<r_base>+[#<a>-#<a_base>]/360*#<stepover>]
			#<r> = [#<ry>]
			o200 while [#<r> LT #<r_end>]
				G1 X[#<cx>+#<ix>] Y[#<cy>-#<ry>]
				#<ix> = [#<ix> + 0.01]
				#<a> = ATAN[-#<ry>]/[#<ix>]
				#<r> = sqrt[[#<ix>]**2 + [#<ry>]**2]
				#<r_end> = [#<n_end>*#<stepover> + #<r_base>+[#<a>-#<a_base>]/360*#<stepover>]
				;#<r_end> = [FUP[sqrt[[#<ix>]**2 + [#<ry>]**2]/#<stepover>]*#<stepover>]

				o204 if [#<ix> GT #<rx>]
					#<run> = 0
					o200 BREAK
				o204 endif
			o200 endwhile

			#<lastx> = [COS[#<a>]*#<r>]

			o201 while [SIN[#<a>]*#<r> LT #<ry>]
				#<xtmp> = [COS[#<a>]*#<r>]
				o205 if [#<xtmp> GT #<rx>]
					#<xtmp> = [#<rx>]
				o205 endif
				G1 X[#<cx>+#<xtmp>] Y[#<cy>+SIN[#<a>]*#<r>]

				#<a> = [#<a> + 1]
				#<r> = [#<n_end>*#<stepover> + #<r_base>+[#<a>-#<a_base>]/360*#<stepover>]

				
			o201 endwhile


			G1 X[#<cx>+#<lastxx>] Y[#<cy>+#<ry>]

			G0 Z#<z_lift>
			G0 X[#<cx>+#<lastx>] Y[#<cy>-#<ry>]

			#<ix> = #<lastx>
			#<lastxx> = [COS[#<a>]*#<r>]
		o202 endwhile
	o<xp> endsub

M2