Ported a function and optimized. Readbility needs to be taken care of