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

/** \file */

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

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

    // Make a sensor
    Plat::Sensor sensor;
    
    /* Shot */
    FCam::Shot stream1;
    // Set the shot parameters
    stream1.exposure = 50000;
    stream1.gain = 1.0f;

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

    // Enable the histogram unit.
    stream1.histogram.regions = 1;
    stream1.histogram.region[0] = FCam::Rect(0, 0, 640, 480);
    
    // We will stream until the exposure stabilizes.
    int count = 0;        // # of frames streamed
    int stableCount = 0;  // # of consecutive frames with stable exposure.
    float exposure;       // exposure for the current frame
    float lastExposure;   // exposure for the previous frame
    
    FCam::Frame frame;

    do {
        // Update lastExposure
        lastExposure = exposure;
    
        // Ask the sensor to stream with the given parameters.
        sensor.stream(stream1);
    
        // Retrieve a frame
        frame = sensor.getFrame();
        assert(frame.shot().id == stream1.id); // Check the source of the request.
    
        // Call the autoexposure algorithm. It will update stream1.
        autoExpose(&stream1, frame);
    
        // Calculate the exposure used
        exposure = stream1.exposure * stream1.gain;
    
        // Increment stableCount if the exposure is within 10% of the previous one.
        if (count++ > 0) {            
            if (fabs((exposure - lastExposure) / lastExposure < 0.1f)) {
                stableCount++;
            } else {
                stableCount = 0;
            }
        }
    } while (stableCount < 5); // Terminate when stable for 5 frames.
    
    // Order the sensor to stop streaming.
    sensor.stopStreaming();
    printf("Processed %d frames until stable for 5 frames!\n", count);
    printf("Final exposure: %f us\n", exposure);
    
    // 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);
}