Fix #289 and small optimizes