Few additions

Added test project and updated the algorithm a bit.