first working version of the HQP solver