New code, not complete yet. Just add some more functions so the main program can work