First working version: need some clean up

Signed-off-by: Marcello Angileri <marcelloangileri@gmail.com>