Help from Nathan van Geffen, worked further on box collision detection and made it work almost perfectly.