-- Scan one line along x axis, at ypos position.
-- Values are sent to console in LDraw units (floating point values)

function ScanLineX (dx, ypos)
  local xpos, curtacho, probeval, sign
   abort=0
 AbortMessage()
 Degree2Ldu = nxt.float(96,0)
 if(XMin > XMax) then 
  sign=-1
 else
  sign=1
 end
 _,curtacho = nxt.OutputGetStatus(XMotor)
 if(( XMin-curtacho ) > 0) then
  nxt.OutputSetSpeed( XMotor, 32, 100, XMin-curtacho )
 else
  nxt.OutputSetSpeed( XMotor, 32, -100, curtacho-XMin )
 end
 repeat
  _,curtacho = nxt.OutputGetStatus(XMotor)
  collectgarbage()
 until ( nxt.abs(curtacho-XMin ) < 4)
 delay(400)
 
 for xpos = XMin, XMax, dx*sign do
  _,curtacho = nxt.OutputGetStatus(XMotor)
  nxt.OutputSetSpeed( XMotor, 32, 30*sign, dx )
  probeval = Probe()
  _,curtacho = nxt.OutputGetStatus(XMotor)
  p1 = {}
  collectgarbage() 
  p1.x=nxt.float(curtacho,0) / Degree2Ldu
  p1.y=nxt.float(ypos,0) / Degree2Ldu
  p1.z=nxt.float(probeval,0) / Degree2Ldu
  print("v " .. p1.x .. " " .. p1.y .. " " .. p1.z)
  if 8 == nxt.ButtonRead() then
   nxt.SoundTone(1000,100,1)
   repeat until 1 ~= nxt.ButtonRead()
   nxt.DisplayClear()
   nxt.DisplayText("Grey button",6 ,8)
   nxt.DisplayText("to confirm",6 ,16)
   nxt.SoundTone(400,600,1)
   delay(600);
   repeat until 0 ~= nxt.ButtonRead()
   if 1 == nxt.ButtonRead() then
    nxt.DisplayClear()
    nxt.DisplayText("Scan aborted",6 ,8)
    abort=1
    break
   end
   repeat until 0 == nxt.ButtonRead()
   AbortMessage()    
  end
 end
 return abort
end

-- Scan one face of the object with a (dx, dy) resolution
function ScanXY (dx, dy)
 local ypos, curtacho, sign
 if(YMin > YMax) then 
  sign=-1
 else
  sign=1
 end
 _,curtacho = nxt.OutputGetStatus(YMotor)
 nxt.OutputSetRegulation( XMotor, 1, 1 ) 
 nxt.OutputSetRegulation( YMotor, 1, 1 )
 nxt.OutputSetRegulation( ProbeMotor, 1, 1 ) 
 if(( YMin-curtacho ) > 0) then
  nxt.OutputSetSpeed( YMotor, 32, 100, YMin-curtacho )
 else
  nxt.OutputSetSpeed( YMotor, 32, -100, curtacho-YMin )
 end
 repeat
  _,curtacho = nxt.OutputGetStatus(YMotor)
 until ( nxt.abs(curtacho-YMin ) < 4)
 delay(400)
  
 for ypos = YMin, YMax, dy*sign do
  _,curtacho = nxt.OutputGetStatus(YMotor)
  nxt.OutputSetSpeed( YMotor, 32, 30*sign, dy )
  if (ScanLineX(dx, curtacho)==1) then
   break
  end
 end
end

-- Display scan abort message
function AbortMessage()
    nxt.DisplayClear()
    nxt.DisplayText("Keep orange",6 ,8)
    nxt.DisplayText("button pressed",6 ,16)
    nxt.DisplayText("to abort scan",6 ,24)
end

-- Delay function 
function delay(ms)
local curtick
curtick=nxt.TimerRead()
repeat
until ((nxt.TimerRead()-curtick)>ms)
end

-- Moves the probe till it meets object or reach near boundary.
function Probe()
 local curtacho, tachoreturn
 nxt.OutputSetSpeed( ProbeMotor, 32, -40 )
 repeat
  _,curtacho = nxt.OutputGetStatus(ProbeMotor)
  if ( nxt.InputGetStatus(LightSensor) > LightThreshold ) then break end
 until ( curtacho < 0 )
 nxt.OutputSetSpeed( ProbeMotor, 32, 100, ProbeMax-curtacho )
 repeat
  _,tachoreturn = nxt.OutputGetStatus(ProbeMotor)
 until ( tachoreturn > ProbeMax )
 if ( curtacho < 0 ) then return nil else return curtacho end
end

-- Suppress scanning functions from memory to make room for scan setup functions.
function ForgetScan()
 ScanLineX=nil
 Probe=nil
 delay=nil
 AbortMessage=nil
 ScanXY=nil
 collectgarbage()
end