#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <FCam/N900.h>
#include <FCam/AutoFocus.h>

/** \file */

// Select the platform
namespace Plat = FCam::N900;
// namespace Plat = FCam::F2;

/***********************************************************/
/* Autofocus                                               */
/*                                                         */
/* This example shows how to request streams and deal with */
/* the incoming frames, and also uses the provided         */
/* autofocus routine.                                      */
/***********************************************************/
int main(int argc, char ** argv) {

    /* Devices */
    Plat::Sensor sensor;
    Plat::Lens lens;
    sensor.attach(&lens); // Attach the lens to the sensor.

    /* Autofocus supplied by FCam API */
    FCam::AutoFocus autoFocus(&lens);

    /* Shot */
    FCam::Shot stream1;
    // Set the shot parameters
    stream1.exposure = 50000;
    stream1.frameTime = 50000;
    stream1.gain = 1.0f;

    // Request a resolution, and allocate storage. 
    stream1.image = FCam::Image(640, 480, FCam::UYVY);

    // Enable the sharpness unit.
    stream1.sharpness.enabled = true;
    stream1.sharpness.size = FCam::Size(16, 12);

    // We will stream until the focus stabilizes.
    int count = 0;        // # of frames streamed

    // Order the sensor to stream.
    sensor.stream(stream1);

    // Ask the autofocus algorithm to start sweeping the lens.
    autoFocus.startSweep();

    // Stream until autofocus algorithm completes.
    FCam::Frame frame;

    do {
        // Retrieve a frame
        frame = sensor.getFrame();
        assert(frame.shot().id == stream1.id); // Check the source of the request.
      
        // Call the autofocus algorithm.
        autoFocus.update(frame);
      
        // Increment frame counter.
        count++;
    } while (!autoFocus.idle());

    // Order the sensor to stop streaming.
    sensor.stopStreaming();
    printf("Processed %d frames until autofocus completed!\n", count);

    // There may still be shots in the pipeline. Consume them.
    while (sensor.shotsPending() > 0) frame = sensor.getFrame();

    // Write out file if needed. 
    if (argc > 1) FCam::saveJPEG(frame, argv[1]);

    // Check that the pipeline is empty.
    assert(sensor.framesPending() == 0);
    assert(sensor.shotsPending() == 0);
}