Highlighting current section in navbar

I have a scrolling page with a navbar and I want the section the user is scrolling through to be highlighted in the navbar. At the moment it almost accomplishes this, but is highlighting the incorrect link.

A demonstration is at http://codepen.io/meek/pen/NNprYb?editors=1000

The code that does this is as follows:

// highlight current tab
    $(window).on("scroll", function() {

        var currentPos = $(window).scrollTop();

        $('.nav li a').each(function() {
            var sectionLink = $(this);
            var section = $(sectionLink.attr('href'));
            if(section.position().top <= currentPos && sectionLink.offset().top + section.height() >= currentPos) {
                $('.nav li').removeClass('active');
                sectionLink.parent().addClass('active');
            }
            else {
                sectionLink.parent().removeClass('active');
            }
        });

    });

I've tried several things, but can't get it to reliably add the active class to the correct session. Help appreciated!

edit: to be clearer, the problem is that it's only highlighting the section when you've scrolled a bit into it, instead of right at the top, meaning that when you click a section to scroll to the top of that section automatically, that section is not highlighted.

edit2: So changing the if statement to:

if(currentPos + $('#nav-wrapper').outerHeight() >= section.position().top && currentPos + $('#nav-wrapper').outerHeight() <= sectionLink.offset().top + section.outerHeight()) {

has made an improvement although not completely fixed the issue. The home, about and portfolio sections all highlight the correct link but not contact.