New update

Refactor codes to make it a bit more clean and can now be compile. Added
median filter but found it that make it really slow...