Algorithm changed with an efficient math equation (not finished)