Cleaned code a bit, made new makefile, untested