Rewriting Algorithm

Rewriting how things work. It can go down now. Kind of have a problem
where the elevator gets stuck on a floor now...