Cleaned code and restrutured. Added to TODO