Finished the SBA* algo up to compilable status, needs debugging though.