Cleaned up code and reformated. Minimal optimizations