src/F2/Daemon.cpp

#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/fcntl.h>
#include <sys/ioctl.h>
#include <poll.h>
#include <string.h>
#include <linux/videodev2.h>

#include "FCam/Time.h"
#include "FCam/Action.h"

#include "Daemon.h"
#include "../Debug.h"

// local copy of kernel headers - keep in sync!
#warning Do not forget to update the mt9p031.h header when changing the kernel!
#include "linux/mt9p031.h"

namespace FCam { namespace F2 {

    void *daemon_setter_thread_(void *arg) {
        Daemon *d = (Daemon *)arg;
        d->runSetter();    
        d->setterRunning = false;    
        pthread_exit(NULL);
    } 

    void *daemon_handler_thread_(void *arg) {
        Daemon *d = (Daemon *)arg;
        d->runHandler();
        d->handlerRunning = false;
        pthread_exit(NULL);    
    }

    void *daemon_action_thread_(void *arg) {
        Daemon *d = (Daemon *)arg;
        d->runAction();
        d->actionRunning = false;
        pthread_exit(NULL);
    }

    Daemon::Daemon(Sensor *_sensor) :
        sensor(_sensor),
        stop(false), 
        frameLimit(128),
        dropPolicy(FCam::Sensor::DropNewest),
        setterRunning(false), 
        handlerRunning(false), 
        actionRunning(false),
        waitingForFirstRequest(true),
        debugMode(false) {

        // tie ourselves to the correct sensor
        v4l2Sensor = V4L2Sensor::instance("/dev/video0");

        // launch the daemon thread with realtime priority

        // check I'm root
        if (getuid()) {
            printf("F2 camera daemon can only run as root. Aborting.\n");
            return;
        }
    
        // enable real-time scheduling modes
        FILE *f = fopen("/proc/sys/kernel/sched_rt_runtime_us", "w");
        if (f) {
            fprintf(f, "-1");
            fclose(f);
        } else {
            printf("Could not enable real-time scheduling modes, daemon thread creating may fail\n");
        }

        // make the mutexes for the producer-consumer queues
        if ((errno = 
             -(pthread_mutex_init(&actionQueueMutex, NULL) ||
               pthread_mutex_init(&cameraMutex, NULL)))) {
            perror("Error creating mutexes");
        }
    
        // make the semaphores
        sem_init(&actionQueueSemaphore, 0, 0);

        // make the semaphore for pipeline flushes
        pipelineFlush = true;
    
    }

    void Daemon::launchThreads() {

        pthread_attr_t attr;
        struct sched_param param;

        // make the setter thread

        param.sched_priority = sched_get_priority_min(SCHED_FIFO)+1;

        pthread_attr_init(&attr);

        if ((errno =
             -(pthread_attr_setschedparam(&attr, &param) ||
               pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
               pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
               pthread_create(&setterThread, &attr, daemon_setter_thread_, this)))) {
            perror("Error creating daemon setter thread");
            return;
        } else {
            setterRunning = true;
        }

        // make the handler thread
        param.sched_priority = sched_get_priority_min(SCHED_FIFO);

        if ((errno =
             -(pthread_attr_setschedparam(&attr, &param) ||
               pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
               pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
               pthread_create(&handlerThread, &attr, daemon_handler_thread_, this)))) {
            perror("Error creating daemon handler thread");
            return;
        } else {
            handlerRunning = true;
        }

        // make the actions thread
        param.sched_priority = sched_get_priority_max(SCHED_FIFO);

        if ((errno =
             -(pthread_attr_setschedparam(&attr, &param) ||
               pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
               pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
               pthread_create(&actionThread, &attr, daemon_action_thread_, this)))) {
            perror("Error creating daemon action thread");
            return;
        } else {
            actionRunning = true;
        }

        pthread_attr_destroy(&attr);
    }

    Daemon::~Daemon() {
        stop = true;

        // post a wakeup call to the action thread
        sem_post(&actionQueueSemaphore);

        if (setterRunning) 
            pthread_join(setterThread, NULL);
    
        if (handlerRunning)
            pthread_join(handlerThread, NULL);

        if (actionRunning)
            pthread_join(actionThread, NULL);

        pthread_mutex_destroy(&actionQueueMutex);
        pthread_mutex_destroy(&cameraMutex);

        v4l2Sensor->close();
    }

    void Daemon::setDropPolicy(FCam::Sensor::DropPolicy p, int f) {
        dropPolicy = p;
        frameLimit = f;
        enforceDropPolicy();
    }

    void Daemon::enforceDropPolicy() {
        if (frameQueue.size() > frameLimit) {
            printf("WARNING: frame limit hit (%d), silently dropping %d frames.\n"
                   "You're not draining the frame queue quickly enough. Use longer \n"
                   "frame times or drain the frame queue until empty every time you \n"
                   "call getFrame()\n", frameLimit, frameQueue.size() - frameLimit);
            if (dropPolicy == FCam::Sensor::DropOldest) {
                while (frameQueue.size() >= frameLimit) {
                    _Frame *f = frameQueue.pull();
                    delete f;
                }
            } else if (dropPolicy == FCam::Sensor::DropNewest) {
                while (frameQueue.size() >= frameLimit) {
                    _Frame *f = frameQueue.pullBack();
                    delete f;
                }
            } else {
                printf("Unknown drop policy! Not dropping frames.\n");
            }
        }
    
    }

    void Daemon::debugTiming(bool enable) {
        debugMode = enable;
    }

    void Daemon::runSetter() {

        // The first tick of the universe
        // Lyme disease doesn't even exist yet.
        tickSetter(Time::now());
        while (!stop) {
            //printf("Daemon ioctl hsvs wait\n");
            int err;
            timeval hs_vs_stamp = {0,0};
            v4l2_control ctrl;
            ctrl.id = MT9P031_CID_WAIT_HSVS_1;
            ctrl.value = 0; // Irrelevant for a get
        
            int currentFD = v4l2Sensor->getFD();

            Time now = Time::now();
            //printf("HSVS\n");
            /* Not using sensor->getControl() here because of 
               real possibility of timeout (error condition) */
            if (ioctl(currentFD, VIDIOC_G_CTRL, &ctrl) != 0) {
                printf("HSVS error\n");
            } else {
                if (ctrl.value == -1) {
                    printf("HSVS waiting timeout (probably)\n");
                    hs_vs_stamp.tv_sec = now.s();
                    hs_vs_stamp.tv_usec = now.us();
                } else {
                    //printf("Sec: %d\n", ctrl.value);
                    hs_vs_stamp.tv_sec = ctrl.value;
            
                    ctrl.id = MT9P031_CID_WAIT_HSVS_2;
                    err = ioctl(currentFD, VIDIOC_G_CTRL, &ctrl);
                    if (err != 0) {
                        perror ("Error reading HSVS_2");
                    } else {                
                        hs_vs_stamp.tv_usec = ctrl.value;
                    }
                }
                //printf("USec: %d\n", ctrl.value);
                //printf("Now: %d %d, DeltaT HSVS: %d usec\n", now.s(), now.us(), Time(hs_vs_stamp)-now);
                tickSetter(Time(hs_vs_stamp));          
            }
        }
    
    }

    void Daemon::setReadoutParams(_Frame *req, bool modeSwitch) {

        bool updatedParameters = false;

        // Do atomic write of all the above 
        if (updatedParameters) 
            v4l2Sensor->setControl(MT9P031_CID_WRITE_PARAMS, 0);

    }

    void Daemon::setTimes(_Frame *req, const Time &hs_vs, bool modeSwitch) {
        // how long will the previous frame take to readout
        static int lastReadout = 0; //30000;   
        static bool firstFrame = true;
        int modeSwitchLag;

        if (req) {

            if (firstFrame) {
                modeSwitchLag = req->frameTime;
                firstFrame = false;
            } else {
                modeSwitchLag = modeSwitch ? 20000 + req->frameTime : 0;   
            }

            // Predict when this frame will be done. It should be HS_VS +
            // the readout time for the previous frame + the frame time
            // for this request + however long the ISP takes to process
            // this frame.
            req->processingDoneTime = hs_vs + modeSwitchLag;
        
            // first there's the readout time for the previous frame
            req->processingDoneTime += lastReadout;
        
            // then there's the time to expose and read out this frame
            req->processingDoneTime += req->frameTime;
        
            // then there's some significant time inside the ISP if it's YUV and large
            // (this may be N900 specific)
            int ispTime = 0;
            if (req->image.type() == UYVY) {
                if (req->image.height() > 1024 && req->image.width() > 1024) {
                    ispTime = 60000;
                }
            }
            req->processingDoneTime += ispTime;            
        
            // Also compute when the exposure starts and finishes
            req->exposureStartTime = hs_vs + modeSwitchLag 
                + req->frameTime - req->exposure;

            // Now update the readout time for this frame. This formula
            // is specific to the mt9p031 sensor on the F2.
            if (req->shot().colSkip != prevShot.colSkip ||
                req->shot().colBin != prevShot.colBin ||
                req->shot().rowSkip != prevShot.rowSkip ||
                req->shot().rowBin != prevShot.rowBin) {
                if (req->shot().colSkip > prevShot.colSkip) {
                    //lastReadout = 35000;
                    //lastReadout = 15000;
                    //printf("Increasing skipping, lr: %d\n", lastReadout);
                } else {
                    //lastReadout = 35000;
                    //printf("Decreasing skipping, lr: %d\n", lastReadout);
                }
            } else {
                //lastReadout = 0; //(current.image.size.height > 1008) ? 76000 : 33000;
            }
            //lastReadout = 30000;

            req->exposureEndTime  = req->exposureStartTime + req->exposure + lastReadout;

            dprintf(2,"  Predicted time taken to read out previous frame: %d\n", lastReadout);
            dprintf(2,"  Predicted time to expose and read out this frame: %d\n", req->frameTime);
            dprintf(2,"  Predicted time to take in the ISP: %d\n", ispTime);
            dprintf(2,"  Predicted processingDone: %d %d\n", req->processingDoneTime.s(), req->processingDoneTime.us());
            dprintf(2,"  Predicted exposureEndTime: %d %d\n", req->exposureEndTime.s(), req->exposureEndTime.us());
        

        } else {
            // a bubble!
            //lastReadout = (current.image.size.height > 1008) ? 76000 : 33000;
            //lastReadout = 0;
        }
    }

    void Daemon::setExposureParams(_Frame *req, bool modeSwitch) {
        bool updatedParameters = false;

        // Set ROI parameters
        if (req->shot().rowSkip != current._shot.rowSkip || modeSwitch){
            v4l2Sensor->setControl(MT9P031_CID_ROW_SKIPPING, rowSkipDriver(req->shot().rowSkip) );
            current._shot.rowSkip = req->shot().rowSkip;
            updatedParameters = true;
        }
        if (req->shot().colSkip != current._shot.colSkip || modeSwitch){
            v4l2Sensor->setControl(MT9P031_CID_COL_SKIPPING, colSkipDriver(req->shot().colSkip) );
            current._shot.colSkip = req->shot().colSkip;
            updatedParameters = true;
        }
        if (req->shot().rowBin != current._shot.rowBin || modeSwitch){
            v4l2Sensor->setControl(MT9P031_CID_ROW_BINNING, rowBinDriver(req->shot().rowBin) );
            current._shot.rowBin = req->shot().rowBin;
            updatedParameters = true;
        }
        if (req->shot().colBin != current._shot.colBin || modeSwitch){
            v4l2Sensor->setControl(MT9P031_CID_COL_BINNING, colBinDriver(req->shot().colBin) );
            current._shot.colBin = req->shot().colBin;
            updatedParameters = true;
        }
        if (req->shot().roiCentered ) {
            if (!current._shot.roiCentered || modeSwitch) {
                v4l2Sensor->setControl(MT9P031_CID_ROI_X, 
                                   MT9P031_ROI_AUTO_X);
                v4l2Sensor->setControl(MT9P031_CID_ROI_Y, 
                                   MT9P031_ROI_AUTO_Y);
                updatedParameters = true;
            }
        } else if (current._shot.roiCentered 
                   || req->shot().roiStartX != current._shot.roiStartX 
                   || req->shot().roiStartY != current._shot.roiStartY
                   || modeSwitch) {
            v4l2Sensor->setControl(MT9P031_CID_ROI_X,
                               req->shot().roiStartX);
            v4l2Sensor->setControl(MT9P031_CID_ROI_Y,
                               req->shot().roiStartY);
            updatedParameters = true;
        }
        current._shot.roiCentered = req->shot().roiCentered;
        current._shot.roiStartX = req->shot().roiStartX;
        current._shot.roiStartY = req->shot().roiStartY;

        // Set the exposure and frame time
        if (req->shot().frameTime != current._shot.frameTime
            || modeSwitch) {
            dprintf(2,"new ft: %d\n", req->shot().frameTime);
            v4l2Sensor->setFrameTime(req->shot().frameTime);
            current._shot.frameTime = req->shot().frameTime;
            updatedParameters = true;
        }
        if (req->shot().exposure != current._shot.exposure
            || modeSwitch) {
            dprintf(2,"new exposure: %d\n", req->shot().exposure);
            v4l2Sensor->setExposure(req->shot().exposure);
            current._shot.exposure  = req->shot().exposure;
            updatedParameters = true;
        }

        // Set the gain

        if (req->shot().gain != current._shot.gain || modeSwitch) {
            v4l2Sensor->setGain(req->shot().gain);
            current._shot.gain = req->shot().gain;     
            updatedParameters = true;
        }


        // Send the atomic setting write request
        if (updatedParameters)
            v4l2Sensor->setControl(MT9P031_CID_WRITE_PARAMS, 0);
    
        // Tag the request with the actual params. Also store them in
        // current to avoid unnecessary ioctls (gets never cause I2C).
        current.gain = req->gain = v4l2Sensor->getGain();

        current.exposure  = req->exposure  = v4l2Sensor->getExposure();
        current.frameTime = req->frameTime = v4l2Sensor->getFrameTime();

        dprintf(2,"actual exposure: %d\n", req->exposure);
        dprintf(2,"actual ft: %d\n", req->frameTime);

        current.rowSkip = req->rowSkip = 
            rowSkipFCam(v4l2Sensor->getControl(MT9P031_CID_ROW_SKIPPING));
        current.colSkip = req->colSkip = 
            colSkipFCam(v4l2Sensor->getControl(MT9P031_CID_COL_SKIPPING));
        current.rowBin = req->rowBin = 
            rowBinFCam(v4l2Sensor->getControl(MT9P031_CID_ROW_BINNING));
        current.colBin = req->colBin = 
            colBinFCam(v4l2Sensor->getControl(MT9P031_CID_COL_BINNING));
        current.roiStartX = req->roiStartX =
            v4l2Sensor->getControl(MT9P031_CID_ROI_X);
        current.roiStartX = req->roiStartY =
            v4l2Sensor->getControl(MT9P031_CID_ROI_Y);


        //    printf("  tickSetter took %d us\n", after2-after);

        //printf("Rs: %d, expected ft %d\n", current.rowSkip, current.frameTime);
    }


    void Daemon::tickSetter(Time hs_vs) {
        static _Frame *req = NULL;
        static Time prev_hs_vs;
        static bool wasModeswitch = false;


        //printf("HSVS %d %d, delta %.3f ms\n", hs_vs.s(), hs_vs.us(), (hs_vs-prev_hs_vs)/1000.0);
    repeat_modeswitch:
        prev_hs_vs = hs_vs;

        // Step 1 - finish dealing with second half of last request (final parameters, actions)

        usleep(1000); // Get past real HSVS

        // The MT9P031 apparently needs all its parameters set at the same time. This isn't per spec,
        // but does appear to work.  
        if (req) {
            //printf("TS: Setting sensor params\n");        
        }

        // setTimes must be done after setExposureParams, or old time estimates will be used
        setTimes(req, hs_vs, wasModeswitch);
        wasModeswitch = false;

        if (req) {
            dprintf(1,"TS: Pushing to inflight\n");
            // set the sensor readout parameters and predicted done time on the pending request
            // and then push it onto the handler's input queue and the v4l2 buffer queue       
            //setReadoutParams(req);

            // now queue up this request's actions
            pthread_mutex_lock(&actionQueueMutex);
            for (std::set<FCam::Action*>::iterator i = req->shot().actions.begin();
                 i != req->shot().actions.end();
                 i++) {
                Action a;
                a.time = req->exposureStartTime + (*i)->time - (*i)->latency;
                a.action = (*i)->copy();
                actionQueue.push(a);
            }
            pthread_mutex_unlock(&actionQueueMutex);
            for (size_t i = 0; i < req->shot().actions.size(); i++) {
                sem_post(&actionQueueSemaphore);
            }

            inFlightQueue.push(req);
            req = NULL;
        } else {
            // an unpredictable bubble, so do nothing
        }

        // Step 2 - Start processing next request (exposure, frame time, mode switches)
        // If the queue is empty, try to generate some requests from a stream
        if (!requestQueue.size()) {
            dprintf(1,"TS: Asking the sensor to generate a request\n");
            sensor->generateRequest();
        }

        if (requestQueue.size()) {
            dprintf(1,"TS: Grabbing next request \n");
            // peek at the next request
            req = requestQueue.front();
        } else {
            // There are no pending requests, push a bubble into the
            // pipeline. The default parameters for a frame work nicely as
            // a bubble (as short as possible, no stats generated), except 
            // that it should be unwanted.
            dprintf(1,"TS: Creating a bubble request\n");
            _Frame *req = new _Frame;

            req->_shot.wanted = false;

            // bubbles should just run at whatever resolution is going. If
            // fast mode switches were possible, it might be nice to run
            // at the minimum resolution to go even faster, but they're
            // not.
            req->_shot.image = Image(current._shot.image.size(), current._shot.image.type(), Image::Discard);

            // Temporary hack - keep the same exposure to keep everything flowing
            // smoothly.  This is bad for long exposure bubbles!
            req->_shot.exposure = current._shot.exposure;

            // generate histograms and sharpness maps if they're going,
            // but drop the data.
            req->_shot.histogram  = current._shot.histogram;
            req->_shot.sharpness  = current._shot.sharpness;

            // push the bubble into the pipe
            requestQueue.push(req);
        }
        prevShot = current._shot; // Keep around the settings for one more go-around

        //printf("TS: Checking mode switch\n");
        // Check if the next request requires a mode switch
        if (req->shot().image.size() != current._shot.image.size() ||
            req->shot().image.type() != current._shot.image.type() ||
            req->shot().histogram  != current._shot.histogram  ||
            req->shot().sharpness  != current._shot.sharpness) {

            printf("TS:   Mode Requested %d %d\n", req->shot().image.width(), req->shot().image.height());
            printf("TS:   Mode Current %d %d\n", current._shot.image.width(), current._shot.image.height());
        
            // flush the pipeline
            dprintf(1,"Setter: Mode switch required - flushing pipe\n");
            pipelineFlush = true;

            pthread_mutex_lock(&cameraMutex);
            dprintf(1,"Setter: Handler done flushing pipe, passing control back to setter\n");
        
            // do the mode switch
        
            if (current.image.width() > 0) {
                printf("Setter: Stopping camera for mode switch\n");
                v4l2Sensor->stopStreaming();
                v4l2Sensor->close();
                v4l2Sensor->open();
            } else {
                printf("Setter: Opening camera for the first time\n");
                v4l2Sensor->open();
            }

            // Ensure we're in direct control mode (sensor doesn't auto-update parameters on frame bounds)
            v4l2Sensor->setControl(MT9P031_CID_DIRECT_MODE, 1);
        
            // Set parameters before starting stream
            setReadoutParams(req, true);
            setExposureParams(req, true);

            // set all the params for the new frame
            V4L2Sensor::Mode m;
            m.width  = req->shot().image.width();
            m.height = req->shot().image.height();
            m.type   = req->shot().image.type();
        
            v4l2Sensor->startStreaming(m, req->shot().histogram, req->shot().sharpness);

            /* Read this again because we actually told the sensor to switch modes,
               and only now does it know its new resolution */
            current.frameTime = req->frameTime = v4l2Sensor->getFrameTime();
            dprintf(1,"Setter: First frame time %f ms\n", current.frameTime/1000.);

            hs_vs = Time::now();
            dprintf(1,"Setter: Setter done bringing up camera, passing control back to handler\n");
            pipelineFlush = false;
            pthread_mutex_unlock(&cameraMutex);
        
            m = v4l2Sensor->getMode();
            req->image = Image(m.width, m.height, m.type, Image::Discard);
        
            current._shot.image = req->shot().image;
            current._shot.histogram  = req->shot().histogram;
            current._shot.sharpness  = req->shot().sharpness;

            current.image = req->image;

            dprintf(2,"TS: Popping mode switch request\n");
            // pop the request      
            requestQueue.pop();

            dprintf(1,"TS: Mode switch done at %d %d, delta %f ms\n", hs_vs.s(), hs_vs.us(), (hs_vs-prev_hs_vs)/1000.0);
            //hs_vs += req->frame_time;
            wasModeswitch = true;
            goto repeat_modeswitch;
        } 
    
        dprintf(2,"Setter: no mode switch required\n");
        Time b = Time::now();
        setReadoutParams(req);
        setExposureParams(req);
        Time a = Time::now();
        dprintf(2,"    Time to set %f ms\n", (a - b) / 1000.0);

        dprintf(2,"TS: Popping request\n");
        // pop the request 
        requestQueue.pop();

        req->image = current.image;
        //req->histogram = req->shot().histogram;
        //req->sharpness = req->shot().sharpness;

        // Exposure and frame time are set. Return and wait for the next
        // HS_VS before setting readout parameters for this request. (and pulling the next request).
        //usleep(500);

    }


    void Daemon::runHandler() {
        _Frame *req = NULL;
        V4L2Sensor::V4L2Frame *f = NULL;
        Time prevDoneTime = Time::now();

        pthread_mutex_lock(&cameraMutex);

        while (!stop) {

            // the setter may be waiting for me to finish processing
            // outstanding requests
            if (!req && pipelineFlush && inFlightQueue.size() == 0) { 
                printf("Handler: Handler done flushing pipe, passing control back to setter\n");
                while (pipelineFlush) {
                    pthread_mutex_unlock(&cameraMutex);
                    // let the setter grab the mutex. It has higher priority,
                    // so it should happen instantly. We put this in a while
                    // loop just to be sure.
                    usleep(10000);
                    pthread_mutex_lock(&cameraMutex);
                }
                printf("Handler: Setter done bringing up camera, passing control back to handler\n");
            
            }
        
            if (pipelineFlush) {
                printf("Handler: Setter would like me to flush the pipeline, but I have requests in flight\n");
            }
        
            // wait for a frame
            if (!f) {
                f = v4l2Sensor->acquireFrame(true);
            }

            if (!f) {
                printf("Handler got a NULL frame\n");
                usleep(10000);
                continue;
            } 

            dprintf(2,"Handler: frame %d %d, delta %f\n",
                    f->processingDoneTime.s(), f->processingDoneTime.us(),
                    (f->processingDoneTime-prevDoneTime)/1000.);
            prevDoneTime = f->processingDoneTime;

            // grab a request to match to it
            if (!req) {
                if (inFlightQueue.size()) {
                    //.printf("Handler: popping a frame request\n");
                    req = inFlightQueue.pull();
                } else {
                    // there's no request for this frame - probably coming up
                    // from a mode switch or starting up
                    //printf("Handler: Got a frame without an outstanding request. Waiting some for one.\n");
                    int timeout = 5;
                    while (timeout > 0 && (inFlightQueue.size()==0) ) {
                        timeout--;
                        usleep(1000);
                    }
                    if (timeout > 0) {
                        req = inFlightQueue.pull();
                    } else {
                        printf("  Giving up on waiting for request\n");
                        v4l2Sensor->releaseFrame(f);
                        f = NULL;
                        continue;
                    }
                }
            }

            dprintf(1,"Handler:     Expected at %d %d, delta %f ms\n", 
                    req->processingDoneTime.s(), req->processingDoneTime.us(),
                    (f->processingDoneTime - req->processingDoneTime)/1000.);
        
            // at this point we have a frame and a request, now look at
            // the time delta between them to see if they're a match
            int dt = req->processingDoneTime - f->processingDoneTime;

            if (dt < -25000 && !debugMode) { // more than 25 ms late
                dprintf(0,"Handler: Expected a frame at %d %d, but one didn't arrive until %d %d, dt %f ms\n",
                       req->processingDoneTime.s(), req->processingDoneTime.us(),
                       f->processingDoneTime.s(), f->processingDoneTime.us(),
                       dt/1000.0);
                req->image = Image(req->image.size(), req->image.type(), Image::Discard);
                if (!req->shot().wanted) {
                    /* Get rid of bubble allocated by setter */
                    delete req;
                } else {
                    // the histogram and sharpness map may still have appeared
                    req->histogram = v4l2Sensor->getHistogram(req->exposureEndTime-2300, 
                                                              req->shot().histogram);
                    req->sharpness = v4l2Sensor->getSharpnessMap(req->exposureEndTime-2300,
                                                                 req->shot().sharpness);

                    frameQueue.push(req);
                    enforceDropPolicy();
                }
                req = NULL;
            } else if (dt < 15000 || debugMode) { // In debugMode, accept all comers
                // Is this frame wanted or a bubble?
                if (!req->shot().wanted) {
                    // it's a bubble - drop it
                    //printf("Handler: discarding a bubble\n");
                    delete req;
                    v4l2Sensor->releaseFrame(f);
                } else {
                
                    // this looks like a match - bag and tag it
                    req->processingDoneTime = f->processingDoneTime;

                    size_t bytes = req->image.width()*req->image.height()*2;
                    if (f->length < bytes) bytes = f->length;
                
                    if (req->shot().image.autoAllocate()) {
                        req->image = Image(req->image.size(), req->image.type(),f->data).copy();
                        dprintf(2,"Autoallocate: %d x %d, %d\n", 
                                req->image.width(), req->image.height(), req->image(0,0));
                    } else if (req->shot().image.discard()) {
                        req->image = Image(req->image.size(), req->image.type(), Image::Discard);
                    } else {
                        if (req->image.size() != req->shot().image.size()) {
                            dprintf(0,"ERROR: Requested image size (%d x %d) "
                                    "on an already allocated image does not "
                                    "match actual image size (%d x %d). Dropping image data.\n",
                                   req->shot().image.width(), req->shot().image.height(),
                                   req->image.width(), req->image.height());
                            req->image = Image(req->image.size(), req->image.type(), Image::Discard);
                        } else { // the size matches
                            req->image = req->shot().image;
                            // Need to decide on a more dynamic timeout for lock() here,
                            // but 10ms shouldn't be too bad
                            if(req->image.lock(10000)) {
                                req->image.copyFrom(Image(req->image.size(), req->image.type(),f->data));
                                req->image.unlock();
                            }  else {
                                dprintf(0,"WARNING: Daemon discarding image data (Target is locked)\n");
                                req->image = Image(req->image.size(), req->image.type(), Image::Discard);
                            }
                        }
                    }

                    v4l2Sensor->releaseFrame(f);
                    req->histogram = v4l2Sensor->getHistogram(req->exposureEndTime-2300, 
                                                              req->shot().histogram);
                    req->sharpness = v4l2Sensor->getSharpnessMap(req->exposureEndTime-2300, 
                                                                 req->shot().sharpness);

                    frameQueue.push(req);
                    enforceDropPolicy();

                
                    //              printf("Handler: finalized a request for a %d x %d frame\n", req->image.size.width, req->image.size.height);            
                }
            
                req = NULL;
                f = NULL;

            } else { // more than 10ms early... something weird's going on
                dprintf(0,"Handler: Received an early mystery frame (%d %d) vs (%d %d), dropping it.\n",
                       f->processingDoneTime.s(), f->processingDoneTime.us(),
                       req->processingDoneTime.s(), req->processingDoneTime.us());
                v4l2Sensor->releaseFrame(f);
                f = NULL;
            }
        
        }
        pthread_mutex_unlock(&cameraMutex);
    
    }


    void Daemon::runAction() {
        printf("Action thread running...\n");
        while (1) {       
            sem_wait(&actionQueueSemaphore);
            if (stop) break;
            // priority inversion :(
            pthread_mutex_lock(&actionQueueMutex);
            Action a = actionQueue.top();
            actionQueue.pop();
            pthread_mutex_unlock(&actionQueueMutex);

            Time t = Time::now();
            int delay = (a.time - t) - 500;
            if (delay > 0) usleep(delay);
            Time before = Time::now();
            // busy wait until go time
            while (a.time > before) before = Time::now();
            a.action->doAction();
            //Time after = Time::now();
            printf("Action thread: Initiated action %d us after scheduled time\n", before - a.time);
            delete a.action;
        }
    }

} }