przepisanie reszty kodu z matlaba i naprawienie robaka