Fixed bugs and removed all TODO comments

Signed-off-by: Tabor Kvasnicka <kamakwazee@gmail.com>