cpp is still wrong, but at least matlab is working pretty well