Improved code, some test pending