Created debug case. Transfer implementation from header to source. Fix some error but still broken...(Commit before my laptop running out of power...)
Merge remote-tracking branch 'origin/master' into test_code