Added a function (plot for test)
Some small modifications