Finish castle.cpp