Finish objective function
Add minimal test