Theta* Algorithm now working

New line of sight algorithm works properly.
More methods added for debugging.