void AutonomousMovementDirector::run()
{
    uint32_t sound_sensor_left_value, sound_sensor_right_value;
    while (1) {
        respond_to_sound();
        respond_to_obstacle();
        movementController->move_forward(speed);
    }
}

void AutonomousMovementDirector::respond_to_sound()
{
    uint32_t sound_sensor_left_value = 0;
    uint32_t sound_sensor_right_value = 0;

    if (
        SoundSensor::get_sound_sensor_left()->queue.receive(&sound_sensor_left_value, 0)
        || SoundSensor::get_sound_sensor_right()->queue.receive(&sound_sensor_right_value, 0)
    ) {
        if (sound_sensor_left_value > sound_sensor_right_value) {
            movementController->turn_right(speed);
            delay(500);
        } else if (sound_sensor_left_value > sound_sensor_right_value) {
            movementController->turn_left(speed);
            delay(500);
        }
    }
}

#define MS_PER_DIRECTION_OFFSET (10)
void AutonomousMovementDirector::respond_to_obstacle()
{
    float distance_in_cm;
    if (UltrasonicSensor::get_instance()->queue.receive(&distance_in_cm, 0)) {
        if (distance_in_cm > 0) {
            int8_t direction_offset = find_open_place();

            if (direction_offset < 0) {
                movementController->turn_left(speed/2);
                delay((uint16_t) MS_PER_DIRECTION_OFFSET * (-1 * direction_offset));
            } else {
                movementController->turn_right(speed/2);
                delay((uint16_t) MS_PER_DIRECTION_OFFSET * direction_offset);
            }
        }
    }
}

/*
 * Using the ultrasonic sensor that's attached to the servo, the closest direction that's not blocked is found.
 *
 * Returns the offset to that direction a negative number means it's to the left.
 */
int8_t AutonomousMovementDirector::find_open_place()
{
    float distance_in_cm = 0;
    int8_t direction_offset = -100;

    UltrasonicSensor* ultrasonicSensor = UltrasonicSensor::get_instance();

    ultrasonicSensor->suspend(); // stop continuously polling
    ultrasonicSensor->queue.receive(&distance_in_cm, 400);  // wait for current distance measurement to finish

    for (int8_t i=-90; i <= 90; i += 20) {
        if (abs(i) >= abs(direction_offset)) // the closest open place has already been found, searching further is useless
            break;

        Servo::get_instance()->set_position(i + 90);
        delay(200);

        ultrasonicSensor->send_pulse(); // start distance measurement
        ultrasonicSensor->queue.receive(&distance_in_cm, 400);  // wait until measurement is complete and get the result

        if (distance_in_cm == 0)  // distance == 0 means distance > 30
            direction_offset = i;
    }

    return direction_offset;
}