Adding in-house implementation of CG. Does not compile