Added additional code

Almost finished the searching algorithm and improved the main function