Complete c solution with tests