Updated algorithm

Includes better way to handle rotation. However some updates are
required to be put again. xbtsw will fix soon.