Added testing ( see run_test.sh. Code improved quite a bit :) )