Improved algo. Working version. But still draft
Moved algo to header file. Used header in my vis