Algorithm Part III - 3

- changed the way of storing matrices
- fixed some logical error