-- This is a very simple EXAMPLE navigation program, which avoids obstacles using the Braitenberg algorithm

if (sim_call_type==sim_childscriptcall_initialization) then 
	
	myconsole=simAuxiliaryConsoleOpen('Sensors',1000,0,{0.5,0.9},{0.2,0.2},NULL,NULL)
    simAuxiliaryConsoleShow(myconsole,1)

	usensors={-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1}
	for i=1,16,1 do
		usensors[i]=simGetObjectHandle("Pioneer_p3dx_ultrasonicSensor"..i)
	end
	motorLeft=simGetObjectHandle("Pioneer_p3dx_leftMotor")
	motorRight=simGetObjectHandle("Pioneer_p3dx_rightMotor")
	noDetectionDist=0.5
	maxDetectionDist=0.2
	detect={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}
	braitenbergL={-0.2,-0.4,-0.6,-0.8,-1,-1.2,-1.4,-1.6, 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}
	braitenbergR={-1.6,-1.4,-1.2,-1,-0.8,-0.6,-0.4,-0.2, 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}
	v0=2
	dist1=0.0
end 

if (sim_call_type==sim_childscriptcall_cleanup) then 
  
end 

if (sim_call_type==sim_childscriptcall_actuation) then 
	for i=1,16,1 do
		res,dist=simReadProximitySensor(usensors[i])
		
		if (res>0) and (dist<noDetectionDist) then
			if (dist<maxDetectionDist) then
				dist=maxDetectionDist
			end
			detect[i]=1-((dist-maxDetectionDist)/(noDetectionDist-maxDetectionDist))
		else
			detect[i]=0
		end
	end

	for i=1,16,1 do
		simAuxiliaryConsolePrint(myconsole,"S[")
		simAuxiliaryConsolePrint(myconsole,i)
		simAuxiliaryConsolePrint(myconsole,"]=")

		res,dist=simReadProximitySensor(usensors[i])
		-- string.format("%1.4f",dist)
        if (res > 0) then 
			simAuxiliaryConsolePrint(myconsole,string.format("%1.4f",dist))
			simAuxiliaryConsolePrint(myconsole," ")	
        else
			simAuxiliaryConsolePrint(myconsole,"0.0    ")
		end
	end
	simAuxiliaryConsolePrint(myconsole,"\n")	

	vLeft=v0
	vRight=v0
	
	for i=1,16,1 do
		vLeft=vLeft+braitenbergL[i]*detect[i]
		vRight=vRight+braitenbergR[i]*detect[i]
	end
	
	simSetJointTargetVelocity(motorLeft,vLeft)
	simSetJointTargetVelocity(motorRight,vRight)
end