Fix IndexOutOfBound and other enhancements

Please pull if you interested.