#include "main.h"

bool lineCheck(U16 verLimit, U16 lineIntensity, U16 lineLowerIntensity, U16 lineEndIntensity, U16 lineScanThickness, U16 & verPosition, U16 & leftWidth, U16 & rightWidth)
{
    U16     st = 0, y, yy, x = sensor.horPixel() / 2, wl, wr, h, Y;
    Pixel   pixt, pixb;
    bool    first = false, ret = false, ldone, rdone, l, r;
    // Grab an image
    camera.grab(image);
    
    // Find line position from the bottom to the top
    for(y = sensor.verPixel() - 1; y > verLimit; y--)
    {
        pixt = image.pixel(x, y);
        if(!first)
        {
            if(pixt.word > lineIntensity)
            {
                first = true;
                st = y; // Vertical line position
            }
        }
        else
            if(pixt.word < lineLowerIntensity)
                break;
    }
    // At this point y is the last position of the line
    if(st > verLimit && first)
    {
        ret = true;
        y = st + (y - st) / 2;  // Center of the line
        wl = wr = 0;
        h = sensor.horPixel() / 2;
        l = r = true;
        for(x = 0; x < (otus->_lineScanRange / 2); x++)
        {
            // Right check
            rdone = false;
            // Adjust for line distorsion
            Y = y + x * otus->_lineScanRightAdj;
            if(r)
                for(yy = y - lineScanThickness; yy < y + lineScanThickness; yy++)
                {
                    pixb = image.pixel(h + x, yy);
                    if(pixb.word > lineEndIntensity)
                    {
                        wr++;
                        rdone = true;
                        break;
                    }
                }
            if(!rdone) r = false;   // If not found any pixel disable this loop
            
            // Left check
            ldone = false;
            // Adjust for line distorsion
            Y = y + x * otus->_lineScanLeftAdj;
            if(l)
                for(yy = y - lineScanThickness; yy < y + lineScanThickness; yy++)
                {
                    pixt = image.pixel(h - x, yy);
                    if(pixt.word > lineEndIntensity)
                    {
                        wl++;
                        ldone = true;
                        break;
                    }
                }
            if(!ldone) l = false;   // If not found any pixel disable this loop
                
            if(!ldone && !rdone)
                break;
        }
        verPosition = y;
        leftWidth = wl;
        rightWidth = wr;
    }
    else
    {
        verPosition = 0;
        leftWidth = 0;
        rightWidth = 0;
    }
    return ret;
}

void lineScanProcess()
{
    TimeMark(TL_LINE_SCAN);
    if(lineCheck(otus->_verLimit, otus->_lineIntensity, otus->_lineLowerIntensity, otus->_lineEndIntensity, otus->_lineScanThickness, telemetry->_verPosition, telemetry->_leftWidth, telemetry->_rightWidth))
    {
        telemetry->_lineScanBench = TimeMeasure(TL_LINE_SCAN);
        if(otus->_lineScanDebug) printf("At = %3u, Left = %3u, Right = %3u, ET: %3u ms\n", telemetry->_verPosition, telemetry->_leftWidth, telemetry->_rightWidth, telemetry->_lineScanBench);
        otus->_lineScanValid = true;
    }
    else
        otus->_lineScanValid = false;
}

void lineScanInit()
{
    otus->_lineScanLeftAdj = (F32) otus->_lineScanLeftDrift / (F32) otus->_lineScanRange;
    otus->_lineScanRightAdj = (F32) otus->_lineScanRightDrift / (F32) otus->_lineScanRange;
    otus->_lineScanLeftAdj *= -1.0f;
    otus->_lineScanRightAdj *= -1.0f;
}

bool saveImage(U8 fileExtension, int quality)
{
    bool    r = false;
    int     year, month, day;
    U8      hh, mm, ss;
    char    fn[64];
    RtcGetDate(&year, &month, &day);
    RtcGetTime(&hh, &mm, &ss);
    // Create dir
    sprintf(fn, "DCIM/%04u%02u%02u", year, month, day);
    file.mkdir(fn);
    // Create file name path
    sprintf(fn, "DCIM/%04u%02u%02u/%02u%02u%02u.", year, month, day, hh, mm, ss);
    switch(fileExtension)
    {
        case FE_RAW:    _strcat(fn, "RAW");
                        r = imgFile.save(fn, &image, 0);
                        break;
        case FE_JPG:    _strcat(fn, "JPG");
#if(IMAGE_DEPTH == 16)
                        imgVar.create(sensor.horPixel(), sensor.verPixel(), 1, otImage::IMG_U8);
                        image.shrink(imgVar, 0, 4);
                        r = imgFile.save(fn, &imgVar, 0, quality);
                        imgVar.close();
#else
                        r = imgFile.save(fn, &image, 0, quality);
#endif
                        break;
        case FE_AVI:    _strcat(fn, "AVI");
                        break;
    }
    //printf("File: %s = %d\n", fn, r);
    return r;
}

bool timeLapse(U32 nx, U32 it, U8 ff, int qy)
{
    bool    r;
    ElapsedTimeStart(TIMER0);
    for(U32 n = 0; n < nx; n++)
    {
        while(ElapsedTimeEnd(TIMER0) < it)
            camera.grab(image);
        ElapsedTimeStart(TIMER0);
        r = saveImage(ff, qy);
        if(!r) break;
    }
    return r;
}