Algorithm implemented but not optimized. Further tests needed