Some work towards issue #542

@libardo  could you kindly code review when you have a moment?