Reapplied pull request c387bf7 from cy8aer and did some code cleaning