# Copyright 2010 by Nicholas Peshman 

# THIS SOFTWARE IS PROVIDED "AS IS" AND WITHOUT ANY EXPRESS OR
# IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.

# Name :          GCode Gen 3D 1.0
# Description :   Creates GCode conforming to the contour of a model for 3 Axis machines
# Author :        Nicholas Peshman makingfoamfly@gmail.com
# Usage :         Move Group or groups to within the safe area starting from 0,0 and on the 0 plane then run the contour script
# Date :          23.Nov.2010
# Type :			tool
# History :

require 'sketchup.rb'

class GCodeGen3D

	def initialize
		@mod = Sketchup.active_model
		@ents = @mod.entities
	
		# the starting values
		@spindle = PhlatScript.spindleSpeed #8000
		@feedRate = PhlatScript.feedRate.to_i #100
		@BitDiam = PhlatScript.bitDiameter.to_f #0.125
		@MatThick = PhlatScript.materialThickness.to_f #2.0
		@SafeLength = PhlatScript.safeWidth.to_f  #42.0
		@SafeWidth = PhlatScript.safeHeight.to_f#22.0
		@MultiPass = PhlatScript.useMultipass? # PhlatScript.multipassEnabled #false
		@MultiPassDepth = PhlatScript.multipassDepth.to_f #0.1
		@OvercutPercent = PhlatScript.cutFactor #1.40
		@Phlat_SafeHeight = PhlatScript.safeTravel.to_f #2.50
        @SafeHeight = (@Phlat_SafeHeight.to_f + @MatThick.to_f)
		
		@SafeXOffset = 0.0
		@SafeYOffset = 0.0
		@modelMaxX = 0.0
		@modelMaxY = 0.0
		@modelMaxZ = 0.0
		@modelMinX = 99999.0
		@modelMinY = 99999.0
		@modelMinZ = 99999.0
		
		@FileToSave = "D:/test.cnc"

		#Calculated Values
		@BitOffset = @BitDiam/2
		@stepOver = @BitOffset
		@modelgrid = Array.new
		@gCodegrid = Array.new
		@verticalgrid = Array.new
		@optimizedgrid = Array.new
		@GCodeOffset = 50
	end
	
	def modelgrid
		@modelgrid
	end
	def gCodegrid
		@gCodegrid
	end
	def vgrid
		@verticalgrid
	end
	
   def generate
   
   
		#Get stepover as a parameter in percent of BitDiameter
		prompts = ["Enter StepOver as Percentage of Bit Diameter"]
		defaults = ["30"]
		results = inputbox prompts, defaults, "StepOver Percentage"
		@stepOver = @BitDiam * ( results[0].to_f/100)
		
		

		#need to Cycle through all entities finding the minimum and max points if inside the safe area 
		#Safe the max X,Y, and Z
		if(enter_file_dialog(@mod))
		
			Sketchup.status_text = "Starting Phlat 3D script" 
			puts "(A 3D Contour)"
		puts "(StepOver: #@stepOver)"
		puts "(Spindle speed: #@spindle)"
		puts "(FeedRate: #@feedRate)"
		puts "(Bit Diameter: #@BitDiam)" 
		puts "(Material Thickness: #@MatThick)"
		puts "(Safe Length: #@SafeLength)"
		puts "(Safe Width: #@SafeWidth)"
		puts "(Multipass: #@MultiPass)"
		puts "(Multipass Depth: #@MultiPassDepth)"
		puts "(OverCut: #@OvercutPercent)"
		puts "(SafeHeight: #@SafeHeight)"
		puts "BitOffset: #@BitOffset"
		
			for ent in @ents
				if (@SafeXOffset < ent.bounds.min.x) and (@SafeYOffset < ent.bounds.min.y) and (@SafeXOffset+@SafeLength > ent.bounds.max.x) and (@SafeYOffset + @SafeWidth > ent.bounds.max.y)
					@modelMaxX = ent.bounds.max.x if ent.bounds.max.x > @modelMaxX
					@modelMaxY = ent.bounds.max.y if ent.bounds.max.y > @modelMaxY
					@modelMaxZ = ent.bounds.max.z if ent.bounds.max.z > @modelMaxZ
					@modelMinX = ent.bounds.min.x if ent.bounds.min.x  < @modelMinX
					@modelMinY = ent.bounds.min.y if ent.bounds.min.y  < @modelMinY
					@modelMinZ = ent.bounds.min.z if ent.bounds.min.z  < @modelMinZ
					
				end
			end
			
			outsideSafe = false
			
				
			
			if !outsideSafe
				#Now Get the model values at the grid points
				generateModelGrid
				
				#Do Vertical anaylysis and manip
				generateVerticals

				#now determine any interference with the model and adjust grid with appropriate offset
				generateGCodeGrid
				
				#optimize GCode
				optimizeGCodeGrid(@gCodegrid)
				
				#output the GCode
				if @MultiPass
					printGCodeInterval(@optimizedgrid) #@gCodegrid) @optimizedgrid)
				else
					printGCode(@optimizedgrid)
				end
				
				
                @basefloor.erase!

			end
		end
		
		UI::messagebox("3D GCode Generation Finished")
	end

	def optimizeGCodeGrid(grid)
		puts "Starting to Optimize GCode"
		Sketchup.status_text = "Starting to Optimize GCode"
		i = 1
		pt = grid[0]
		@optimizedgrid += [pt]
		removedpt = false
		rise = 0
		run = 0
		
		while i < (grid.length-1)
		if removedpt
		else
			prevpt = grid[i-1]
		end
			
			curpt = grid[i]
			nextpt = grid[i+1]
			
			if curpt != grid[i-1] and curpt != nextpt
				if curpt != nil and prevpt != nil and nextpt != nil
					if (curpt.x == prevpt.x) and (curpt.x == nextpt.x)
				
							if not removedpt
								rise = curpt.z - prevpt.z
								run = curpt.y - prevpt.y
							end
						
							if run != 0
								slope = rise/run
								const = prevpt.z - (slope*prevpt.y)
								
								if nextpt.z != (slope * nextpt.y) + const
									@optimizedgrid+= [curpt]
									removedpt = false
								else
									removedpt = true
								end
							else
								if prevpt.y != nextpt.y
									@optimizedgrid+= [curpt]
									removedpt = false
								end
							end
					else
						if curpt != nil
							@optimizedgrid += [curpt]
							removedpt = false
						end
					end	
				else
					if curpt != nil
						@optimizedgrid += [curpt]
						removedpt = false
					end
					
				end
			else
				if curpt != nil and curpt != grid[i-1]
						@optimizedgrid += [curpt]
						removedpt = false
				end
			end
			i += 1
		
		end
		@optimizedgrid += [grid[grid.length-1]]
		puts "Finished Optimize GCode"
		Sketchup.status_text = "Finished Optimize GCode"
	end
	def generateVerticals
		
		puts "Starting to generate Verticals"
		Sketchup.status_text = "Starting to generate Verticals"
		i = 0
		prevpt = @modelgrid[0]
		curpt = @modelgrid[0]
		nextpt = @modelgrid[1]


		while i < @modelgrid.length
			if i == 0
				curpt = @modelgrid[i]
				nextpt = @modelgrid[i+1]
			elsif i == (@modelgrid.length) -1 
				prevpt = @modelgrid[i-1]
				curpt = @modelgrid[i]
				nextpt = @modelgrid[i]
			else
				prevpt = @modelgrid[i-1]
				curpt = @modelgrid[i]
				nextpt = @modelgrid[i+1]			
			end
			
			if curpt != nil 
				if nextpt != nil
					if curpt.x == nextpt.x

						zdelta = nextpt.z - curpt.z

						if zdelta.abs > @stepOver							
							#puts "Adding in a vertical at #{curpt}"
							
							@verticalgrid += [curpt]

							if zdelta > 0
								zpos = curpt.z 
								while zpos <= nextpt.z
									testpt = Geom::Point3d.new(curpt.x, curpt.y , zpos)
									vector = Geom::Vector3d.new(0,1,0)
									if nextpt.y < curpt.y
										#puts "Increase Z with decrease y"
										vector = Geom::Vector3d.new(0,-1,0)
										#puts "#{testpt}, #{vector}"
									end
									newpt = findModelIntersection(testpt, vector)
									#puts "#{newpt}"
									if newpt != nil
										if (newpt.y - curpt.y).abs < @stepOver
											
											@verticalgrid += [newpt]
										end
									end
									zpos += @stepOver
								end
								testpt = Geom::Point3d.new(curpt.x, curpt.y , nextpt.z)
									vector = Geom::Vector3d.new(0,1,0)
									if nextpt.y < curpt.y
										#puts "Increase Z with decrease y"
										vector = Geom::Vector3d.new(0,-1,0)
										#puts "#{testpt}, #{vector}"
									end
									newpt = findModelIntersection(testpt, vector)
									#puts "#{newpt}"
									if newpt != nil
										if (newpt.y - curpt.y).abs < @stepOver
											
										@verticalgrid += [newpt]
										end
									end
									zpos += @stepOver
							
							elsif zdelta < 0
								zpos = curpt.z 
								while zpos >= nextpt.z
									testpt = Geom::Point3d.new(nextpt.x, nextpt.y , zpos)
									vector = Geom::Vector3d.new(0,-1,0)
									if nextpt.y < curpt.y
										vector = Geom::Vector3d.new(0,1,0)
									end
									newpt = findModelIntersection(testpt, vector)

									if newpt != nil
									if (newpt.y - curpt.y).abs < @stepOver
										@verticalgrid += [newpt]
										end
									end
									zpos -= @stepOver
								end
								testpt = Geom::Point3d.new(nextpt.x, nextpt.y , nextpt.z)
									vector = Geom::Vector3d.new(0,-1,0)
									if nextpt.y < curpt.y
										vector = Geom::Vector3d.new(0,1,0)
									end
									newpt = findModelIntersection(testpt, vector)

									if newpt != nil
										if (newpt.y - curpt.y).abs < @stepOver
											@verticalgrid += [newpt]
										end
									end
							end

						else
							@verticalgrid += [curpt]

						end

					else
						@verticalgrid += [curpt]
					end	
				
				else
					@verticalgrid += [curpt]
				end			
			end
			
			i+=1

		end
		puts "Finished generating Verticals"
		Sketchup.status_text = "Finished generating Verticals"

	end
	
	def findModelIntersection(point, vector)
		if point != nil and vector != nil
			ray = [point, vector]
			colpt = @mod.raytest ray
			if colpt != nil
				return colpt[0]
			else
				return nil
			end
		else
			return nil
		end
	end

	def round_to(value, x)
		(value * 10**x).round.to_f / 10**x
	end

	def printGCodeInterval(grid)
		puts "Writing File #@FileToSave"
		Sketchup.status_text = "Writing File #@FileToSave"
		nf = File.new @FileToSave, "w+"
		nf.puts "%"
		nf.puts "(A 3D Contour)"
		nf.puts "(StepOver: #@stepOver)"
		nf.puts "(Spindle speed: #@spindle)"
		nf.puts "(FeedRate: #@feedRate)"
		nf.puts "(Bit Diameter: #@BitDiam)" 
		nf.puts "(Material Thickness: #@MatThick)"
		nf.puts "(Safe Length: #@SafeLength)"
		nf.puts "(Safe Width: #@SafeWidth)"
		nf.puts "(Multipass: #@MultiPass)"
		nf.puts "(Multipass Depth: #@MultiPassDepth)"
		nf.puts "(OverCut: #@OvercutPercent)"
		nf.puts "(SafeHeight: #@SafeHeight)"
		nf.puts "G90 G20 G49"
		curz = 0.0 - @MultiPassDepth
		while curz > (0.0 - @MatThick - @MultiPassDepth)
			nf.puts "M3 S#@spindle"
			nf.puts "G0 Z#@Phlat_SafeHeight"
			
			xval = sprintf("%f",round_to(grid[0].to_a[0],5))
			yval = sprintf("%f",round_to(grid[0].to_a[1],5))
			zval = sprintf("%f",round_to(grid[0].to_a[2],5))
			nf.puts "X#{xval} Y#{yval}"		
			nf.puts "G1 Z#{zval} F#@feedRate"
			
			for point in grid
				xval = sprintf("%f",round_to(point.to_a[0], 5))
				yval = sprintf("%f",round_to(point.to_a[1], 5))
				zval = sprintf("%f",round_to(point.to_a[2], 5))
			
				if zval.to_f > curz
					#zval = round_to(point.to_a[2], 5)
					nf.puts "X#{xval} Y#{yval} Z#{zval}"
				else
					scurz = sprintf("%f",round_to(curz,5))
					nf.puts "X#{xval} Y#{yval} Z#{scurz}"
				end
			end
			nf.puts "M05"
			nf.puts "Z#@Phlat_SafeHeight"
			nf.puts "G0 X0 Y0"
			curz -= @MultiPassDepth
		end
		
		nf.puts "M30"
		nf.puts "%"
		nf.close
		puts "File finished writing"
		Sketchup.status_text = "File finished writing"
	end	
	def printGCode(grid)
		puts "Writing File #@FileToSave"
		Sketchup.status_text = "Writing File #@FileToSave"
		nf = File.new @FileToSave, "w+"
		nf.puts "%"
		nf.puts "(A 3D Contour)"		
		nf.puts "(StepOver: #@stepOver)"
		nf.puts "(Spindle speed: #@spindle)"
		nf.puts "(FeedRate: #@feedRate)"
		nf.puts "(Bit Diameter: #@BitDiam)" 
		nf.puts "(Material Thickness: #@MatThick)"
		nf.puts "(Safe Length: #@SafeLength)"
		nf.puts "(Safe Width: #@SafeWidth)"
		nf.puts "(Multipass: #@MultiPass)"
		nf.puts "(Multipass Depth: #@MultiPassDepth)"
		nf.puts "(OverCut: #@OvercutPercent)"
		nf.puts "(SafeHeight: #@SafeHeight)"
		nf.puts "G90 G20 G49"
		nf.puts "M3 S#@spindle"
		nf.puts "G0 Z#@Phlat_SafeHeight"
		xval = sprintf("%f",round_to(grid[0].to_a[0],5))
		yval = sprintf("%f",round_to(grid[0].to_a[1],5))
		zval = sprintf("%f",round_to(grid[0].to_a[2],5))
		nf.puts "X#{xval} Y#{yval}"		
		nf.puts "G1 Z#{zval} F#@feedRate"
		for point in grid
			xval = sprintf("%f",round_to(point.to_a[0], 5))
			yval = sprintf("%f",round_to(point.to_a[1], 5))
			zval = sprintf("%f",round_to(point.to_a[2], 5))
			nf.puts "X#{xval} Y#{yval} Z#{zval}"
		end
		nf.puts "M05"
		nf.puts "Z#@Phlat_SafeHeight"
		nf.puts "G0 X0 Y0"
		nf.puts "M30"
		nf.puts "%"
		nf.close
		puts "File finished writing"
		Sketchup.status_text = "File finished writing"
	end	
	def generateGCodeGrid
		
		#need to cycle through the modelgrid for each point
		puts "Started generating G Code"
		Sketchup.status_text = "Started generating G Code"
		oldpoint = @verticalgrid[0]
		for point in @verticalgrid
			if oldpoint != point
				hitarray = Array.new
				alreadyAdjusted = Array.new
				#point = adjustpoint2(point, hitarray, alreadyAdjusted, false)
				#point = adjustpoint(point, oldpoint, false)
				if point != nil
					point.z -= @MatThick
					@gCodegrid += [point]
				end
			end
			oldpoint = point
		end
		
		puts "Finished generating G Code"
		Sketchup.status_text = "Finished generating G Code"
	end
	
	def adjustpoint2(point, hitarray, alreadyAdjusted, retest)
	
		if point != nil
			if point.z < @MatThick
				acollision = false
				hitarray.clear
				north = determineBitCollision point, Geom::Vector3d.new(0,1,0)
				south = determineBitCollision point, Geom::Vector3d.new(0,-1,0)
				east = determineBitCollision point, Geom::Vector3d.new(1,0,0)
				west = determineBitCollision point, Geom::Vector3d.new(-1,0,0)
				#fill out array with hit points then get the unique ones
				if north[0]
					hitarray += ["North"]
					acollision = true
				end
				if south[0]
					hitarray += ["South"]
					acollision = true
				end
				if east[0]
					hitarray += ["East"]
					acollision = true
				end
				if west[0]
					hitarray += ["West"]
					acollision = true
				end
				
				if acollision
				
					hitarray = hitarray.uniq
					operatedOn = false
					#puts "#{hitarray}"
					
					#Now move point based on collisoins in the hit array
					if hitarray.include?("North") and hitarray.include?("South")
						#puts "prior #{hitarray}"
						point.z += @stepOver
						hitarray.clear
						alreadyAdjusted.clear
						#puts "post #{hitarray}"
						operatedOn = false
						adjustpoint2(point, hitarray,alreadyAdjusted, true)
						
					elsif hitarray.include?("North")
						if not alreadyAdjusted.include?("North")
							point.y -= (@BitOffset - north[1].abs)
							alreadyAdjusted += ["North"]
							operatedOn = true
							adjustpoint2(point, hitarray,alreadyAdjusted, true)		
							
						end
					elsif hitarray.include?("South")
						#puts "#{point} #{south}"
						if not alreadyAdjusted.include?("South")
							point.y += (@BitOffset - south[1].abs)
							alreadyAdjusted += ["South"]
							operatedOn = true
							adjustpoint2(point, hitarray,alreadyAdjusted, true)
						end
					end
					
					
					if hitarray.include?("West") and hitarray.include?("East")
						point.z += @stepOver
						hitarray.clear
						alreadyAdjusted.clear
						operatedOn = false
						adjustpoint2(point, hitarray,alreadyAdjusted, true)
					elsif hitarray.include?("East")
						if not alreadyAdjusted.include?("East")
							point.x -= (@BitOffset - east[1].abs)
							alreadyAdjusted += ["East"]
							operatedOn = true
							adjustpoint2(point, hitarray,alreadyAdjusted, true)
						end
					elsif hitarray.include?("West")
						if not alreadyAdjusted.include?("West")
							point.x += (@BitOffset - west[1].abs)	
							alreadyAdjusted += ["West"]
							operatedOn = true
							adjustpoint2(point, hitarray,alreadyAdjusted, true)
						end
					end
					
					#colpt = findModelIntersection(point, Geom::Vector3d.new(0,0,1))
					#if colpt != nil
					#	point.z = colpt.z
					#end
					if not operatedOn
						return point
					end
					
				else
					#puts "No Collision Found"
					return point
				end
			else
				puts "point higher than thickness"
				return point
				
			end
		else
			puts "Nil encounered"
		end
	end
	

	
	def determineBitCollision (point, vector)
		#determines if the bit used will eat out a chunk of the physical model. Done by a ray test and then returning if a collision occures
		#ray = [point, vector]
		colpt = findModelIntersection(point, vector)
		if colpt == nil
			return [false,0]
		else
			distance = colpt.distance point
			if distance > (@BitOffset - 0.001) #The reduction in bit offset is needed so that we don't always return nil after a retest 
				return [false,distance]
			else
				#if distance < 0.001
					#return [false,distance]
				#else
					return [true, distance]
				#end
			end
		end
	end
	
	
	def generateModelGrid
			currposx = @modelMinX
			currposy = @modelMinY
			ydir = 1
			Sketchup.status_text = "Starting Generating Model Grid"
			puts "BitOffset: #@BitOffset"
			puts "modelMaxX: #@modelMaxX"
			puts "modelMaxY: #@modelMaxY"
			puts "modelMaxZ: #@modelMaxZ"
			puts "modelMinX: #@modelMinX"
			puts "modelMinY: #@modelMinY"
			puts "modelMinZ: #@modelMinZ"
			planval =0 #  @BitDiam * @OvercutPercent
			
            @basefloor = Sketchup.active_model.entities.add_group
            @basefloor.entities.add_face [@modelMinX-5,@modelMinY-5,-planval], [@modelMaxX +5 , @modelMinY-5,-planval], [@modelMaxX+5, @modelMaxY+5,-planval], [@modelMinX-5, @modelMaxY+5,-planval]
			
            while currposx < @modelMaxX
				#The y axis is done this way so the items in the array follow the tool path
				ystarted = true
				#puts "#{ydir}"
				
				while ystarted #currposy < @modelMaxY
					ray = [Geom::Point3d.new(currposx, currposy, 10), Geom::Vector3d.new(0,0,-1)]
					modelpt = @mod.raytest ray
					if modelpt != nil
						@modelgrid += [modelpt[0]]
					end
					currposy += (ydir * @stepOver)
					#puts "#{currposx} , #{currposy}"
					if ydir ==1
						if currposy > @modelMaxY
							ray = [Geom::Point3d.new(currposx, @modelMaxY, 10), Geom::Vector3d.new(0,0,-1)]
							modelpt = @mod.raytest ray
							if modelpt != nil
								@modelgrid += [modelpt[0]]
							end
							ystarted = false
						end
					end
					if ydir == -1
						if currposy < @modelMinY
							ray = [Geom::Point3d.new(currposx, @modelMinY, 10), Geom::Vector3d.new(0,0,-1)]
							modelpt = @mod.raytest ray
							if modelpt != nil
								@modelgrid += [modelpt[0]]
							end
							ystarted = false
						end
					end
				end
								
					currposx += @stepOver
					#currposy = @modelMinY
					#need to flip the y direction for every row
					if ydir == 1
						currposy = @modelMaxY
						ydir = -1
					else
						currposy = @modelMinY
						ydir = 1
					end
			end
			
			puts "Finished generating model grid"
			Sketchup.status_text = "Finished generating model grid"
	end
	
	def enter_file_dialog(model=Sketchup.active_model)
      
	  output_directory_name = PhlatScript.cncFileDir
      output_filename = PhlatScript.cncFileName
      status = false
      result = UI.savepanel(PhlatScript.getString("Save CNC File"), output_directory_name, output_filename)
     if(result != nil)
       # if there isn't a file extension set it to the default
       result += '.' + $default_file_ext if (File.extname(result).empty?)
       PhlatScript.cncFile = result
	   @FileToSave = result
       #PhlatScript.checkParens(result, "Output File")
       status = true
      end
      status
    end
end

#-----------------------------------------------------------------------------
if( not file_loaded?("Phlat3D.rb") )
    label = 'Phlat 3D'
	if $PhlatScript_PlugName
		$PhlatScript_PlugName.add_item(label) { GCodeGen3D.new.generate }
	else
		$PhlatScript_PlugName=UI.menu('Plugins').add_submenu('PhlatPlugins')
		$PhlatScript_PlugName.add_item(label) { GCodeGen3D.new.generate }
	end
end
#-----------------------------------------------------------------------------
file_loaded("Phlat3D.rb")