Add initial CG algorithm