Finished basic algroithm
Fixed turning issue