Working version, need to check accuracy with height >= 7