Progress on castling implementation
Add working castling functionality