Implemented first attempt on algorithm and plotting