-- SCRIPT DO LaserScanner_2D

if (sim_call_type==sim_childscriptcall_initialization) then 
	laserHandle=simGetObjectHandle("LaserScannerLaser_2D")
	jointHandle=simGetObjectHandle("LaserScannerJoint_2D")
	graphHandle=simGetObjectHandle("LaserScannerGraph_2D")
	modelHandle=simGetObjectAssociatedWithScript(sim_handle_self)
	objName=simGetObjectName(modelHandle)
	communicationTube=simTubeOpen(0,objName..'_2D_SCANNER_DATA',1)
--  By FOsorio
    fUsensors = io.open("sick2D.log", "w")
--

end 

if (sim_call_type==sim_childscriptcall_cleanup) then 
	simResetGraph(graphHandle)
    fUsensors:close()
end 

if (sim_call_type==sim_childscriptcall_sensing) then 
	scanningAngle=tonumber(simGetScriptSimulationParameter(sim_handle_self,"scanningAngle"))
	if (scanningAngle==nil) then
		scanningAngle=180
		simSetScriptSimulationParameter(sim_handle_self,"scanningAngle",scanningAngle)
	end
	if (scanningAngle<5) then
		scanningAngle=5
	end
	if (scanningAngle>180) then
		scanningAngle=180
	end
	scanningDensity=tonumber(simGetScriptSimulationParameter(sim_handle_self,"scanningDensity"))
	if (scanningDensity==nil) then
		scanningDensity=2
		simSetScriptSimulationParameter(sim_handle_self,"scanningDensity",scanningDensity)
	end
	if (scanningDensity<0.1) then
		scanningDensity=0.1
	end
	if (scanningDensity>5) then
		scanningDensity=5
	end
	
	simResetGraph(graphHandle)
	pts=scanningAngle*scanningDensity+1
	p=-scanningAngle*math.pi/360
	stepSize=math.pi/(scanningDensity*180)
	points={}
	modelInverseMatrix=simGetInvertedMatrix(simGetObjectMatrix(modelHandle,-1))
	for i=0,pts,1 do
		simSetJointPosition(jointHandle,p)
		p=p+stepSize
		r,dist,pt=simHandleProximitySensor(laserHandle) -- pt is RELATIVE to te rotating laser beam!
		if r>0 then
 -- By  FOsorio
           angaux=p
           distaux=dist
           fUsensors:write(string.format("(%.4f;", angaux*180/math.pi))
           fUsensors:write(string.format("%.4f) ", distaux))
--
			-- We put the RELATIVE coordinate of that point into the table that we will return:
			m=simGetObjectMatrix(laserHandle,-1)
			pt=simMultiplyVector(m,pt)
			pt=simMultiplyVector(modelInverseMatrix,pt) -- after this instruction, pt will be relative to the model base!
			table.insert(points,pt[1])
			table.insert(points,pt[2])
			table.insert(points,pt[3])
		end
		simHandleGraph(graphHandle,0.0)
	end

    -- By FOsorio
    fUsensors:write(string.format("\n"))

	-- Now send the data:
	if #points>0 then
		simTubeWrite(communicationTube,simPackFloats(points))
	end
	
	-- To read the data from another script, use following instructions (in that other script):
	--
	-- INITIALIZATION PHASE:
	-- laserScannerHandle=simGetObjectHandle("LaserScanner_2D")
	-- laserScannerObjectName=simGetObjectName(laserScannerHandle) -- is not necessarily "LaserScanner_2D"!!!
	-- communicationTube=simTubeOpen(0,laserScannerObjectName..'_2D_SCANNER_DATA',1)
	--
	-- TO READ THE DATA:
	-- data=simTubeRead(communicationTube)
	-- if (data) then
	--     laserDetectedPoints=simUnpackFloats(data)
	-- end
	--
	-- laserDetectedPoints is RELATIVE to the model base!
end