Add bulk of project

Add cpp and h source codes and some documentation