Implemented castling

Also created tests for it
TODO: checkmate, removeBlockingStraight