Added Test and main code, first version working