Better visualisation and now tested

Also added a legend