Olá Bruna,
É suficiente calcular a distância do p aos outros pontos e ficar com a menor distância, mas pelo teste o seu programa não está fazendo isso, no PointST ele está retornando:
Symbol table underflow
E no KdTree ele retorna a quantidade errada de pontos em um dos casos.
Da uma olhada na estrutura do seu código para ver se ele está fazendo realmente o que você deseja. Se possível, tenta simular na mão para k = 1, 2, 3, ...
Vê se ele retorna sempre a quantidade certa e se os pontos que ele deveria retornar são os certos.
Obs: Use distanceSquaredTo(), não distanceTo().
Se precisar de ajuda para isso é só chamar
Abraços