-----------------------------------------------------------------------------
-- steveTest.txt - code to implement the NXT benchmark on Steve Hassenplug's
--                 website at:
--
--                  <www.teamhassenplug.org/NXT/NXTSoftwareSpeedTest.html>
-----------------------------------------------------------------------------
-- 2007-03-21 R.Hempel - Original
-----------------------------------------------------------------------------

function SensorLight( port )
  nxt.InputSetType(port,0)
  nxt.InputSetDir(port,1,0)
  nxt.InputSetState(port,1,0)
end

function SensorUS( port )
  nxt.InputSetType(port,2)
  nxt.InputSetDir(port,1,1)
  nxt.InputSetState(port,1,1)
end

function I2CWait( port )
    while true do
       state, txstatus, rxstatus = nxt.I2CGetStatus( port )
      if (0 ~= rxstatus) and (0 ~= txstatus) then
        break
      end
    end
end

function steveTest()
  nxt.DisplayScroll()
  SensorLight(3)
  SensorUS(4)
  nxt.I2CTimer(1)
  nxt.I2CInitPins(4)
  nxt.I2CSendData( 2, string.format( "%c%c%c", 0x02, 0x41, 0x02 ), 3, 0 )
  I2CWait(4)
  startTime = nxt.TimerRead()
  endTime   = startTime + 60000
 
  motorSpeedA = 0
  loopCount = 0
  
  while( endTime > nxt.TimerRead() ) do
    nxt.I2CSendData( 4, string.format( "%c%c", 0x02, 0x42 ), 2, 1 )

    lightSensor = nxt.InputGetState( 3 )
    _,rotationCount = nxt.OutputGetStatus(2)
    
    randomNumber = nxt.random(100,1)
    I2CWait(4)
    
    usSensor = string.byte( nxt.I2CRecvData( 4, 1 ) )
    total = (lightSensor + usSensor + rotationCount)*100/randomNumber
    nxt.OutputSetSpeed( 2, 32, randomNumber )
    nxt.OutputSetSpeed( 3, 32, randomNumber )
    if 50 > randomNumber then
      motorSpeedA = motorSpeedA - 1
    elseif 50 < randomNumber then
      motorSpeedA = motorSpeedA + 1
    end
    
    nxt.DisplayText( motorSpeedA )
    
    loopCount = loopCount + 1
  end

  nxt.OutputSetSpeed( 1, 0, 0 )
  nxt.OutputSetSpeed( 2, 0, 0 )
  nxt.OutputSetSpeed( 3, 0, 0 )

  nxt.DisplayText(loopCount)
end
steveTest()