added angle calculation, changed way of moving and gatefinding, some other fixes