Code overhaul into new format. A few bugs fixed and workarounds implemented (Like OpenCV Bug #1788)