Finished implementation with Seg Fault. Testing required