Added dsp math and algorithm

also added some preliminary tests