Add benchmark, fix safety bugs
Minor fixes