//#DOC Main, program entry and main loop
#include "main.h"
#include <otSerial.h>
#include <otTimer.h>
#include <otPrintf.h>
#include <otCamera.h>
#include <otImageSensor.h>
#include <otImage.h>
#include <otI2cBus.h>
#include <otVMC.h>

otCamera        camera;     // Library for the camera control
otImageSensor   sensor;     // Library for image sensor settings
otImage         image;      // Image store and processing library
otVMC           showData;   // Utiliy to show memory contents

void setup()
{
    // Init connection with the console via UART0
    UartEnableSignals(UART0);
    UartSetBaud(UART0, 115200);
    // Init stout used by printf
    setStdout(4096, UART0);   // printf output is now directed on UART0
    // Init the timer for time measure
    TimerSetAsCounter(TIMER0);
    TimerEnable(TIMER0, true);
    // SMB bus init
    I2cBusReset();
    I2cBusFrequency(100, 30);
    // Wait the console has been started
    DelayMs(1000);      // Wait terminal start
    // Init the standard camera based on MT9V032
    camera.init(otCamera::CM_STANDARD);
    // Init image sensor
    if(!sensor.init(otImageSensor::IS_MT9V032, otImageSensor::MT9V03x_00))
    {
        printf("Can't find image sensor!\n");
        IDLE;
    }
    // Show sensor resolution
    printf("Sensor resolution: %dx%d\n", sensor.horPixel(), sensor.verPixel());
    // Init display utility
    showData.open();
}

void GrabImage(U32 loop, otImage::ImageType fmt, bool aec, F32 exposure = 0.0f)
{
    U32     et;
    // Create the image buffer to store 50 grabbed images 10 bit depth
    image.create(sensor.horPixel(), sensor.verPixel(), 50, fmt);
    // Set Automatic Exposure Control
    sensor.setAec(aec);
    if(!aec)
        // Set the exposure time
        sensor.setExposure(exposure);
    // Start measure of time
    ElapsedTimeStart(TIMER0);
    // Grab n images specified by image
    camera.grab(image);
    // Get elapsed time
    et = ElapsedTimeEnd(TIMER0);
    // Store grab time
    image.setGrabTime(et);
    // Wait comething .... the writings on the console are better read ....
    DelayMs(100);
    // Print some statistics
    printf("[%5u] %3dx%3d, AEC(%d), Word(%d), Frames = %3u, et = %6u ms, fps = %.1f\n", loop, sensor.horPixel(), sensor.verPixel(), aec, image.isWordType(), image.frames(), et, image.fps());
    // Un comment below line to show the memory contents
    //showData.display(image.data(), 1024, otVMC::D_U16);
    // Free image buffer
    image.close();
}

void loop()
{
    static U32 loop = 0;
    
    // Different tests using different binning or one ROI
    
    // 10 bit sampling *********************************************************
    // *************************************************************************
    
    sensor.setBinning(1, 1);
    // Grab images whito AEC on
    GrabImage(loop, otImage::IMG_U16, true);
    // Grab images with AEC off and with an exposure time of 100uS
    GrabImage(loop, otImage::IMG_U16, false, 0.1f);
    
    sensor.setROI(0, 0, 128, 128);
    // Grab images with AEC on
    GrabImage(loop, otImage::IMG_U16, true);
    // Grab images with AEC off and with an exposure time of 100uS
    GrabImage(loop, otImage::IMG_U16, false, 0.1f);
    
    sensor.setBinning(2, 2);
    // Grab images with AEC on
    GrabImage(loop, otImage::IMG_U16, true);
    // Grab images with AEC off and with an exposure time of 100uS
    GrabImage(loop, otImage::IMG_U16, false, 0.1f);
    
    sensor.setBinning(4, 4);
    // Grab images with AEC on
    GrabImage(loop, otImage::IMG_U16, true);
    // Grab images with AEC off and with an exposure time of 100uS
    GrabImage(loop, otImage::IMG_U16, false, 0.1f);
    
    // 8 bit sampling **********************************************************
    // *************************************************************************
    
    sensor.setBinning(1, 1);
    // Grab images with AEC on
    GrabImage(loop, otImage::IMG_U8, true);
    // Grab images with AEC off and with an exposure time of 100uS
    GrabImage(loop, otImage::IMG_U8, false, 0.1f);
    
    sensor.setROI(0, 0, 128, 128);
    // Grab images with AEC on
    GrabImage(loop, otImage::IMG_U8, true);
    // Grab images with AEC off and with an exposure time of 100uS
    GrabImage(loop, otImage::IMG_U8, false, 0.1f);
    
    sensor.setBinning(2, 2);
    // Grab images with AEC on
    GrabImage(loop, otImage::IMG_U8, true);
    // Grab images with AEC off and with an exposure time of 100uS
    GrabImage(loop, otImage::IMG_U8, false, 0.1f);
    
    sensor.setBinning(4, 4);
    // Grab images with AEC on
    GrabImage(loop, otImage::IMG_U8, true);
    // Grab images with AEC off and with an exposure time of 100uS
    GrabImage(loop, otImage::IMG_U8, false, 0.1f);
    
    loop++;
}

/* Program output **************************************************************
 *******************************************************************************

Sensor resolution: 752x480
[    0] 752x480, AEC(1), Word(1), Frames =  50, et =    901 ms, fps = 55.5
[    0] 752x480, AEC(0), Word(1), Frames =  50, et =    902 ms, fps = 55.4
[    0] 128x128, AEC(1), Word(1), Frames =  50, et =    889 ms, fps = 56.2
[    0] 128x128, AEC(0), Word(1), Frames =  50, et =    884 ms, fps = 56.6
[    0] 376x240, AEC(1), Word(1), Frames =  50, et =    739 ms, fps = 67.7
[    0] 376x240, AEC(0), Word(1), Frames =  50, et =    493 ms, fps = 101.4
[    0] 188x120, AEC(1), Word(1), Frames =  50, et =    794 ms, fps = 63.0
[    0] 188x120, AEC(0), Word(1), Frames =  50, et =    293 ms, fps = 170.6
[    0] 752x480, AEC(1), Word(0), Frames =  50, et =    978 ms, fps = 16.8
[    0] 752x480, AEC(0), Word(0), Frames =  50, et =    902 ms, fps = 55.4
[    0] 128x128, AEC(1), Word(0), Frames =  50, et =    889 ms, fps = 56.2
[    0] 128x128, AEC(0), Word(0), Frames =  50, et =    884 ms, fps = 56.6
[    0] 376x240, AEC(1), Word(0), Frames =  50, et =    738 ms, fps = 67.8
[    0] 376x240, AEC(0), Word(0), Frames =  50, et =    491 ms, fps = 101.8
[    0] 188x120, AEC(1), Word(0), Frames =  50, et =    794 ms, fps = 63.0
[    0] 188x120, AEC(0), Word(0), Frames =  50, et =    293 ms, fps = 170.6
[    1] 752x480, AEC(1), Word(1), Frames =  50, et =    978 ms, fps = 16.8
[    1] 752x480, AEC(0), Word(1), Frames =  50, et =    902 ms, fps = 55.4
[    1] 128x128, AEC(1), Word(1), Frames =  50, et =    889 ms, fps = 56.2
[    1] 128x128, AEC(0), Word(1), Frames =  50, et =    884 ms, fps = 56.6
[    1] 376x240, AEC(1), Word(1), Frames =  50, et =    734 ms, fps = 68.1
[    1] 376x240, AEC(0), Word(1), Frames =  50, et =    490 ms, fps = 102.0
[    1] 188x120, AEC(1), Word(1), Frames =  50, et =    794 ms, fps = 63.0
[    1] 188x120, AEC(0), Word(1), Frames =  50, et =    293 ms, fps = 170.6
[    1] 752x480, AEC(1), Word(0), Frames =  50, et =    978 ms, fps = 16.8
[    1] 752x480, AEC(0), Word(0), Frames =  50, et =    902 ms, fps = 55.4
[    1] 128x128, AEC(1), Word(0), Frames =  50, et =    889 ms, fps = 56.2
[    1] 128x128, AEC(0), Word(0), Frames =  50, et =    884 ms, fps = 56.6
[    1] 376x240, AEC(1), Word(0), Frames =  50, et =    805 ms, fps = 62.1
[    1] 376x240, AEC(0), Word(0), Frames =  50, et =    499 ms, fps = 100.2
[    1] 188x120, AEC(1), Word(0), Frames =  50, et =    794 ms, fps = 63.0
[    1] 188x120, AEC(0), Word(0), Frames =  50, et =    293 ms, fps = 170.6
[    2] 752x480, AEC(1), Word(1), Frames =  50, et =    978 ms, fps = 16.8
[    2] 752x480, AEC(0), Word(1), Frames =  50, et =    902 ms, fps = 55.4
[    2] 128x128, AEC(1), Word(1), Frames =  50, et =    889 ms, fps = 56.2
[    2] 128x128, AEC(0), Word(1), Frames =  50, et =    884 ms, fps = 56.6
[    2] 376x240, AEC(1), Word(1), Frames =  50, et =    805 ms, fps = 62.1
[    2] 376x240, AEC(0), Word(1), Frames =  50, et =    499 ms, fps = 100.2
[    2] 188x120, AEC(1), Word(1), Frames =  50, et =    794 ms, fps = 63.0
[    2] 188x120, AEC(0), Word(1), Frames =  50, et =    293 ms, fps = 170.6
[    2] 752x480, AEC(1), Word(0), Frames =  50, et =    978 ms, fps = 16.8
[    2] 752x480, AEC(0), Word(0), Frames =  50, et =    902 ms, fps = 55.4
[    2] 128x128, AEC(1), Word(0), Frames =  50, et =    889 ms, fps = 56.2
[    2] 128x128, AEC(0), Word(0), Frames =  50, et =    884 ms, fps = 56.6
[    2] 376x240, AEC(1), Word(0), Frames =  50, et =    805 ms, fps = 62.1

*/