//#DOC Main, program entry and main loop
#include "main.h"

// Test executed @ 525MHz and 131MHz peripheral clock
// Grab time 22.5 ms @ full resolution
// Data correction 136 ms

bool    dataAlign = false;  // Specify where to do data alignment

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
otImageFile     imgFile;    // Library to save different image file format
otFILE          file;       // File I/O library
otCommandParser parser;     // Console command parser
otSerialThread  io;         // Serial interrupt handler
otUSB           usb;        // Declare USB library
otDAQ           daq;        // Debug

// Console related
FAST_CODE_2 U8 _getChar()
{
    printf("\nPress . or ESC to stop display or any other key to continue\n");
    return io.waitChar();
}

void commandLineInterpreter()
{
    U8  commandLine[256];
    
    if(io.getString(commandLine, 256))
        switch(parser.parse((char *) commandLine))
        {
            case 1: printf("\nBatch exists but is empty: [%s]\n", commandLine);
                    break;
            case 2: printf("\nBatch don't exists: [%s]\n", commandLine);
                    break;
            case 3: printf("\nUnknown command: [%s]\n", commandLine);
                    break;
            case 4: printf("\n???\n");
                    break;
            case 5: // Syntax error
                    break;
        }
    otLegacyDisplayPrompt();
}

// Enable/disable sensor low noise mode
S32 __sensLowNoise(int Argc, char * Argv)
{
    int     mod = _atoi((char *) ARGV(1));
    if(mod)
        sensor.lowNoise(true);
    else
        sensor.lowNoise(false);
    return 1;
}

// Set PYTHON1300 shutter setup
S32 __sensShutter(int Argc, char * Argv)
{
    U16     exp = 1, res = 1, pre = 1;
    
    if(Argc > 1)
        exp = _atoi((char *) ARGV(1));
    if(Argc > 2)
        res = _atoi((char *) ARGV(2));
    if(Argc > 3)
        pre = _atoi((char *) ARGV(3));
    sensor.pythonShutter(exp, res, pre);
}

// Create a local image buffer and remote image server
S32 __imageInit(int Argc, char * Argv)
{
    U16     x = _atoi((char *) ARGV(1));
    U16     y = _atoi((char *) ARGV(2));
    U16     n = _atoi((char *) ARGV(3));
    U8      t = _atoi((char *) ARGV(4));
    U8      it = otImage::IMG_U16;
    bool    wtb = false;
    
    if(image.data())
        image.close();
    
    switch(t)
    {
        case 0: it = otImage::IMG_U8;
                break;
        case 1:
                break;
        case 2: wtb = true;
                break;
    }
    sensor.setROI(0, 0, x, y);
    // Local image buffer
    image.create(sensor.horPixel(), sensor.verPixel(), n, (otImage::ImageType) it);
    // Remote image buffer via DAQ server
    daq.imageCreate(image, wtb);
    return 1;
}

// Command to grab an image(s) and trasfer the contents to desktop via DAQ
FAST_CODE_2 S32 __imageGrab(int Argc, char * Argv)
{
    U32     et;
    
    if(Argc > 1)
    {
        F32     expo;
        expo = _atof((char *) ARGV(1));
        sensor.setExposure(expo);
    }
    ElapsedTimeStart(TIMER0);
    camera.grab(image, TIMER0, dataAlign);
    et = ElapsedTimeEndUs(TIMER0);
    
    // Show the image on desktop
    daq.imageShow(image);
    // Set image delta time on desktop
    daq.imageTimings(et);
    
    return 1;
}

void setup()
{
    // Init stout used by printf
    setStdout(4096, UART0);   // printf output is now directed on UART0
    // Init UART0 handler
    io.init(UART0, 115200);
    // Init the timer for time measure
    TimerSetAsCounter(TIMER0);
    TimerEnable(TIMER0, true);
    
    RtcOn();
    // This test has been written in this date
    RtcSetDate(2021, 8, 18);
    RtcSetTime(12, 0, 0);
   
    // Init the camera based on PYTHON1300
    camera.init(otCamera::CM_TIZIANO);
    // Init image sensor
    sensor.init(otImageSensor::IS_PYTHON1300);

    // Show sensor resolution
    printf("Sensor resolution: %dx%d\n", sensor.horPixel(), sensor.verPixel());
    // Init display utility
    showData.open();
    
    if(file.init())
    {
        file.setStdin(_getChar);
        file.setStdout(printf);
        // If SD is present and ready
        parser.init(PARSER_MAX_AVAILABLE_COMMANDS, &file);
        otLegacyCommandsInit(&parser, &io, &file);
    }
    else
    {
        printf("Can't init SD card!\n");
        // Set command line without SD
        parser.init(PARSER_MAX_AVAILABLE_COMMANDS);
        otLegacyCommandsInit(&parser, &io);
    }
    // Set default procedure for library message
    file.setStdout(printf);
    // Set I/O access
    imgFile.init(&file);
    
    // Init USB channel, the command list can accept 64 different commands
    usb.init(64);
    // Add user USB command here. Use id code starting from 2
    
    // Debug system
    daq.init(&usb);
    
    // x sensor sample clock check
    FpgaPortPxSet(PD_FUNC_1, PD_OUTPUT, PD_OUTPUT, PD_OUTPUT, PD_OUTPUT, PD_OUTPUT, PD_OUTPUT, PD_OUTPUT);
    
    // User commands
    parser.add("IMAGE.NEW" ,    "NI",      __imageInit,     false, 4, 4, "128,1280;128,1024;1,10000;0,2", "horSize verSize frames type; Create a new image server");
    parser.add("IMAGE.GRAB",    "GRAB",    __imageGrab,     false, 0, 1, "",                              "[exposure]; Grab and display an image, default exposure 1ms");
    parser.add("SENSOR.SHUT",   "SS",      __sensShutter,   false, 0, 3, "",                              "[expose] [reset] [prescaler]; Python shutter");
    parser.add("SENSOR.LNOISE", "SN",      __sensLowNoise,  false, 1, 1, "",                              "0 | 1; Python low noise mode enable/disable");
    // Add more user command here ....
    
    // Default sensor setup
    sensor.setROI(0, 0, 1280, 1024);
    image.create(sensor.horPixel(), sensor.verPixel(), 1, otImage::IMG_U16);
    sensor.lowNoise(true);
}

void loop()
{
    // Handle for command line requests
    if(io.ready()) commandLineInterpreter();
    // Handle for USB commands
    usb.process();
}