-- Scan setup function through NXT screen menu and NXT buttons
-- Allow the user to define scan boundaries and calibrate probe light sensor.

function DoSetup()
 MaxMenu=5
 local exit=false
 MenuUsed={" ", " ", " ", " ", " "}
 local item=1
 DisplayMenu(item)
 repeat
  collectgarbage()
    if 2 == nxt.ButtonRead() then
   nxt.SoundTone(1000,30,1)
    item=item+1
   if (item > MaxMenu) then 
    item = 1 
   end
   DisplayMenu(item)
   repeat until 2 ~= nxt.ButtonRead() 
  end
    if 4 == nxt.ButtonRead() then
    nxt.SoundTone(1000,30,1)
    item=item-1
   if (item < 1) then 
    item = MaxMenu 
   end
   DisplayMenu(item)
   repeat until 4 ~= nxt.ButtonRead()
  end
    if 8 == nxt.ButtonRead() then
    nxt.SoundTone(1000,30,1)
   MenuUsed[item]="*"
   DisplayMenu(item)
   if item == 1 then
    ResetX()
    DisplayMenu(item)
   end
   if item == 2 then
    ResetY()
    DisplayMenu(item)
   end
   if item == 3 then
    ResetProbe()
    DisplayMenu(item)
   end
   if item == 4 then
    Calibrate(LightSensor)
    DisplayMenu(item)
   end
   if item == MaxMenu then
    local AllDone=true
    for i=1, MaxMenu do
      if(MenuUsed[i]==" ") then AllDone=false end
    end
    if AllDone then
      exit=true
     nxt.DisplayClear()     
    else
     nxt.DisplayText("Setup not done!",6 ,56)
     nxt.SoundTone(400,600,1)
     delay(600);
     MenuUsed[item]=" "
     DisplayMenu(item)
    end
   end
   repeat until 8 ~= nxt.ButtonRead()
  end
  if 1 == nxt.ButtonRead() then
   nxt.DisplayClear()
   repeat until 4 ~= nxt.ButtonRead()
   AllDone=true
   for i=1, MaxMenu-1 do
    if(MenuUsed[i]==" ") then AllDone=false end
   end
   if AllDone then
    exit=true
   else
    nxt.DisplayText("Setup not done!",6 ,8)
    nxt.SoundTone(400,600,1)
    delay(600);
    nxt.DisplayText("Exit anyway?",6 ,16)
    repeat until 0 ~= nxt.ButtonRead()
    if 8 == nxt.ButtonRead() then
     exit=true
     nxt.DisplayText("            ",6 ,16)
    else
     DisplayMenu(item)
    end
    repeat until 0 == nxt.ButtonRead()
   end
  end
 until exit
 nxt.DisplayText("Setup completed", 0 ,16)
end

-- Move motor port until the orange button is pressed
-- Left arrow decreases speed
-- Right arrow increases speed
-- direction (-1 or 1) allows swapping direction buttons 

function MotorSpeed(port, direction)
  local speed
 local curtick
 nxt.OutputSetRegulation( port, 0, 1 )
 repeat
 until( 8 ~= nxt.ButtonRead() )
  repeat
    if 4 == nxt.ButtonRead() then
   curtick = nxt.TimerRead()
   repeat
     speed = 10 + (nxt.TimerRead() - curtick) / 100
     if speed < 100 then
      nxt.OutputSetSpeed( port, 32, -direction*speed )
     end
     _,curtacho = nxt.OutputGetStatus(port)
     nxt.DisplayText(curtacho.."    ",24, 48)
     collectgarbage()
   until 4 ~= nxt.ButtonRead()
    end

    if 2 == nxt.ButtonRead() then
   curtick = nxt.TimerRead()
   repeat
     speed = 10 + (nxt.TimerRead() - curtick) / 100
     if speed < 100 then
      nxt.OutputSetSpeed( port, 32, direction*speed )
     end
     _,curtacho = nxt.OutputGetStatus(port)
     nxt.DisplayText(curtacho.."    ",24, 48)     
     collectgarbage()
   until 2 ~= nxt.ButtonRead()
    end
    if 0 == nxt.ButtonRead() then
   nxt.OutputSetSpeed( port, 32, 0 )
  end
  until( 8 == nxt.ButtonRead() )
  nxt.OutputSetSpeed( port, 0, 0 )
end

-- Delay function 
function delay(ms)
local curtick
curtick=nxt.TimerRead()
repeat
until ((nxt.TimerRead()-curtick)>ms)
end
 
-- Scan parameters 
XMin = 0
XMax = 5000
XMotor = 2

YMin = 0
YMax = 5000
YMotor = 3

ProbeMotor = 1
ProbeMin = 0
ProbeMax = 5000



-- Set the min boundary of a scan direction.
function SetMin(port, direction)
 local curtacho
 MotorSpeed(port, direction)
 delay(200)
 nxt.SoundTone()
 nxt.OutputResetTacho(port,1,1,1)
 nxt.OutputSetSpeed( port, 32, 0 )
 delay(200)
 _,curtacho = nxt.OutputGetStatus(port)
 nxt.DisplayText(curtacho.."    ",24, 48)
 delay(1000)
 return curtacho
end

-- Set the max boundary of a scan direction.
function SetMax(port, direction)
 local curtacho
 MotorSpeed(port, direction)
 delay(200)
 nxt.SoundTone()
 nxt.OutputSetSpeed( port, 32, 0 )
 _,curtacho = nxt.OutputGetStatus(port)
 nxt.DisplayText(curtacho.."    ",24, 48)
 delay(1000)
 return curtacho
end 


-- Set boundaries of probe movement.
function ResetProbe()
 nxt.DisplayClear()
 nxt.DisplayText("Set Probe near", 0, 8)
 nxt.DisplayText("with < > keys", 0, 16)
 nxt.DisplayText("  then push", 0, 24)
 nxt.DisplayText("orange button", 0, 32)
 ProbeMin = SetMin(ProbeMotor, 1)
 nxt.DisplayText("Set Probe far ", 0, 8) 
 ProbeMax = SetMax(ProbeMotor, 1)
end 


LightSensor=1
LightThreshold=700

-- Set light threshold of probe light sensor
function Calibrate(port)
 local LightVal
 repeat until( 8 ~= nxt.ButtonRead() ) 
 nxt.InputSetType(port,0)
 nxt.InputSetState(port,1,0)
 nxt.InputSetDir(port,1,1)
 nxt.DisplayClear()
 nxt.DisplayText("Let probe on", 0, 8)
 nxt.DisplayText("white zone, push", 0, 16)
 nxt.DisplayText("orange button", 0, 24)
  repeat
    nxt.DisplayText( nxt.InputGetStatus(port).."   ",18,56 )
  until( 8 == nxt.ButtonRead() )
 nxt.SoundTone()
 LightVal=nxt.InputGetStatus(port)
  repeat until( 8 ~= nxt.ButtonRead() ) 
 nxt.DisplayClear()
 nxt.DisplayText("Push probe on", 0, 8)
 nxt.DisplayText("black zone, push", 0, 16)
 nxt.DisplayText("orange button", 0, 24)
  repeat
    nxt.DisplayText( nxt.InputGetStatus(port).."   ",18,56 )
  until( 8 == nxt.ButtonRead() )
 nxt.SoundTone()
 LightThreshold = (nxt.InputGetStatus(port) + LightVal)/2
  repeat until( 8 ~= nxt.ButtonRead() ) 
 nxt.DisplayText( "Thres. ".. LightThreshold,0,56 )
 delay(1000)
end

-- Set boundaries of X movement
function ResetX()
 nxt.DisplayClear()
 nxt.DisplayText("  Set X min", 0, 8)
 nxt.DisplayText("with < > keys", 0, 16)
 nxt.DisplayText("  then push", 0, 24)
 nxt.DisplayText("orange button", 0, 32)
 XMin = SetMin(XMotor, -1)
 nxt.DisplayText("  Set X max", 0, 8)
 XMax = SetMax(XMotor, -1)
end   

-- Set boundaries of Y movement
function ResetY()
 nxt.DisplayClear()
 nxt.DisplayText("  Set Y min", 0, 8)
 nxt.DisplayText("with < > keys", 0, 16)
 nxt.DisplayText("  then push", 0, 24)
 nxt.DisplayText("orange button", 0, 32)
 YMin = SetMin(YMotor, 1)
 nxt.DisplayText("  Set Y max", 0, 8)
 YMax = SetMax(YMotor, 1)
end   

-- Display menu items on NXT screen
function DisplayMenu(entry)
 nxt.DisplayClear()
 nxt.DisplayText("Scanner Setup",6 ,0)
 Menu={"Setup X", "Setup Y/rot", "Setup Probe", "Calibr. LightS.", "Exit"}
 for i=1, MaxMenu do
   nxt.DisplayText( MenuUsed[i], 0,i*8+8, 0 )
  if(i==entry) then
   nxt.DisplayText( Menu[i], 8,i*8+8, 1 )
  else
   nxt.DisplayText( Menu[i], 8,i*8+8, 0 )
  end 
 end

end 


-- This function perform the setup, then free NXT memory of unused function
-- to make room for scanning functions
function Setup()
 collectgarbage()
  DoSetup()
 DisplayMenu=nil
  ResetY=nil
  ResetX=nil
  ResetProbe=nil
 Calibrate=nil
 SetMin=nil
 SetMax=nil
 MotorSpeed=nil
 DoSetup=nil
 collectgarbage()
end