import SoaR

def setup():
    # maxSonarReading is defined in GridStateEstimator
    SoaR.Output.Simulator.MAX_SONAR_DIST = 2.465
    # model the dependence on angle in the simulator
    SoaR.Output.Simulator.SONAR_ANGLE = True
    ## Initialize counters
    robot.stepNo = 0
    robot.inliers = 0
    robot.inlierSum = 0
    robot.inlierMax = 0
    robot.inlierMin = 5.0
    robot.outliers = 0
    robot.outlierSum = 0
    
def step(): 
    sonarI = 3
    thr = 2.4
    numSamples = 30

    if robot.stepNo % numSamples == 0:
        r = None
        ## Print summary
        if robot.inliers > 0:
            inAvg = robot.inlierSum/robot.inliers
            r = [robot.inlierMin, robot.inlierMax]
        else:
            inAvg = None
        if robot.outliers > 0:
            outAvg = robot.outlierSum/robot.outliers
        else:
            outAvg = None
        print robot.stepNo,'in=',robot.inliers,'inAvg=',inAvg, r,'out=',robot.outliers,'outAvg=',outAvg
        ## Reset counters
        robot.inliers = 0
        robot.inlierSum = 0
        robot.inlierMax = 0
        robot.inlierMin = 5.0
        robot.outliers = 0
        robot.outlierSum = 0
    else:
        ## Increment counters
        value = sonarDistances()[sonarI]
        if value > thr:
            robot.outliers += 1
            robot.outlierSum += value
        else:
            robot.inliers += 1
            robot.inlierSum += value
            robot.inlierMin = min(robot.inlierMin, value)
            robot.inlierMax = max(robot.inlierMax, value)
    robot.stepNo += 1

def destroy():
    pass
    
