Finally fixed. Going to clean up code and commit branch to master