Changed repmat to ones for efficiency, other cleanup