Updated algo in s_position.cpp and corrected code for level estimation in main.cpp