Tested and working on matlab