Fixed bug in DirectAlgorithm

Sorry.. -/+ error..
Almost works fine..
More consistency in clusters
If thetas are (almost) equal, forgo the weighing