I just managed to implement both formations and pathfinding.
It's still a bit wonky but man am I happy right now.