C code written but I am still working on efficiency