global [s0_dist s1_dist s2_dist s3_dist s4_dist front_warn turn_mode]

to read_sonar_sensors
    ; Call to the bus device and ask for its four sonar readings

    ; Our first bus write/read will address the Sonar device directly,
    ; the subsiquent bus writes/reads are not addressed to a specific
    ; device, but are needed to read in all the values returned by
    ; the sonar device

    sets0_dist bsr $180
    sets1_dist bsr $181
    sets2_dist bsr $182
    sets3_dist bsr $183
    sets4_dist bsr $184

    if s0_dist < 0
    [
        ab, off
        stop!
    ]

    if s1_dist < 0
    [
        ab, off
        stop!
    ]

    if s2_dist < 0
    [
        ab, off
        stop!
    ]

    if s3_dist < 0
    [
        ab, off
        stop!
    ]

    if s4_dist < 0
    [
        ab, off
        stop!
    ]
end

to main
    b, thatway
    a, thatway
    ab, setpower 8
    ab, on

    setturn_mode 0

    loop
    [
        setfront_warn 0

        read_sonar_sensors

        ifelse s0_dist < 20
        [
           setfront_warn 1
        ]
        [
            ifelse s2_dist < 20
            [
               setfront_warn 1
            ]
            [
                if s3_dist < 20
                [
                   setfront_warn 1
                ]
            ]
        ]

        ifelse front_warn = 1
        [
            beep

            if turn_mode = 0
            [
                setturn_mode 1

                ; Determine if there is more room to the left than the right
                ifelse s1_dist > s4_dist
                [
                    ; left turn
                    b, thisway
                    ab, on
                ]
                [
                    ; right turn
                    a, thisway
                    ab, on
                ]
            ]


        ]
        [
           setturn_mode 0
           b, thatway
           a, thatway
           ab, on
        ]
  ]
end


to main_disabled
    loop
    [
        read_sonar_sensors
        send s0_dist
        ;send s1_dist
        ;send s2_dist
        ;send s3_dist
        ;send 1234
        wait 10
    ]
end

to main_disabledtoo
    a, thatway
    b, thisway

    ab, on
end