Getting close. Need some optimization, cleanup and testing