AutoFocus.cpp

#include "AutoFocus.h"
#include "Lens.h"
#include "Frame.h"

namespace FCam {

    AutoFocus::AutoFocus(Lens *l) : lens(l), state(IDLE) {}
    
    void AutoFocus::startSweep() {
    if (!lens) return;
    state = HOMING;
    // focus at infinity
    lens->setFocus(lens->farFocus());
    stats.clear();
    }

    
  void AutoFocus::update(Frame::Ptr f) {
    if (state == FOCUSED || state == IDLE) return;
    
    if (state == SETTING) {
        if (!lens->focusChanging()) {
        state = FOCUSED;        
        }
        return;
    }
        
    // we're sweeping or homing       
    const Lens::Tags *lensTags = f->tags(lens);
    if (!lensTags) return;
    if (!f->sharpness.valid) return;

    Stats s;
    s.position = lensTags->focus;
    s.sharpness = 0;
    for (int sy = 0; sy < f->sharpness.size.height; sy++) {
        for (int sx = 0; sx < f->sharpness.size.width; sx++) {
        s.sharpness += f->sharpness(sx, sy) >> 10;
        }
    }
    stats.push_back(s);
    //printf("Focus position %f, sharpness %d\n", s.position, s.sharpness);

    if (state == HOMING && !lens->focusChanging()) {
      lens->setFocus(lens->nearFocus(), (lens->nearFocus() - lens->farFocus())*2);
        state = SWEEPING;
        return;
    }

    if (state == SWEEPING && stats.size() > 4) {
        bool gettingWorse = true;

        // check if the sharpness is just getting continuously
        // worse (by at least 1% per tick). If so, don't wait for
        // the sweep to terminate
        for (size_t i = stats.size()-1; i > stats.size()-4; i--) {
        if (stats[i].position < stats[i-1].position ||
            (stats[i].sharpness*101)/100 > stats[i-1].sharpness) {
            gettingWorse = false;
            break;
        }
        }

        // Check if it's time to set the focus
        if (!lens->focusChanging() || gettingWorse) {
        Stats best = stats[0];
        for (size_t i = 1; i < stats.size(); i++) {
            if (stats[i].sharpness > best.sharpness) best = stats[i];
        }

        lens->setFocus(best.position);
        state = SETTING;
        stats.clear();
        return;
        }
    }

    if (state == SETTING && !lens->focusChanging()) {
        state = FOCUSED;
        return;
    }

    }



}

---