BiRRT Working all errors fixed, this code can be used as final