Cleanup of movement code, all works except castling