Refactor code

Refactor furthest current code
Add functions ...