Improved order of operations

Fixed errors and generalized algorithm