debug ARAP energy and implement solver