HCP Code + Test Framework

The HCP code now compiles. Test framework was added to compare results
from python and matlab.