loop fixed other than lane changes
Lane changing logic finished