Add some likely/unlikely
flat some loop
Fix bug (div by 0 and out of bound array)