Fixed bugs with undeclared variables and simplified code

Change-Id: I43c765b55ae4f621388bb263f23a457d7010ea3c