Working implementation, some testing needed