//#DOC Main, program entry and main loop
#include "main.h"
#include <otFPGA.h>
#include <otI2cBus.h>
#include <otSerial.h>
#include <otSpi.h>
#include <otTimer.h>
#include <otPrintf.h>
#include <otSpectrometerShield.h>
#include <otPga.h>
#include <otADS868x.h>
#include <otMalloc.h>
#include <otILI9341.h>
#include <otStatistics.h>
#include <otMemory.h>

enum ADC_CHANNEL : U8 {     // ADC channel usage
    CH_SPECTROMETER = 0,    // CH0: Spectrometer
    CH_PHOTODIODE_BOT = 4,  // CH4: Photodiode (X2) bottom
    CH_PHOTODIODE_TOP = 5   // CH5: Photodiode (X3) top
};

otPga                   pga;    // Multiplexor with programmable gain amplifier
otADS868x               adc;    // 16 bit ADC
otSpectrometerShield    sp;     // Spectrometer
otILI9341               tft;    // Display
otStatistics            stat;   // Statistics app

U16     * buffer, * _blank, * _filtered;

// Convert from pixel number to nm
// Specific for each product
F64 pixelToWavelength(U16 n)
{
    F64 N = n;
    // Polynomial coefficents for 19J00642
    F64 A0 = 3.096332781E2;
    F64 B1 = 2.730661824;
    F64 B2 = -1.528225252E-3;
    F64 B3 = -5.211557344E-6;
    F64 B4 = 1.227033986E-9;
    F64 B5 = 1.538539921E-11;
    F64 nm = A0 + B1 * N +
             B2 * N * N +
             B3 * N * N * N +
             B4 * N * N * N * N +
             B5 * N * N * N * N * N;
    return nm;
}

#define neighbours 1

// Smoothing for spectral data
void smoothing(U16 * input, U16 * output)
{
    int     lenInput = otSpectrometerShield::SPEC_CHANNELS;
    for(int i = 0; i < lenInput; i++)
    {
        float x = 0;
        for(int j = -neighbours; j <= neighbours; j++)
        {
            if((i + j <= 0) || (i + j >= lenInput))
                x += input[i];
            else
                x += input[i + j];
        }
        output[i] = x / (neighbours * 2 + 1);
    }
}

// Draw range of sampled data and wavelength peak
void plotRange(U16 mn, U16 mx, F64 nm)
{
    char    buf[32];
    U16     col = RGB565_GREEN;
    U32     l = (U32) mx;
    // Blank previous draw
    tft.fillRect(288, 0, 319, 239, RGB565_BLACK);
    l = 240 * (mx - mn) / 25000;
    // 15%
    if(l < 36) col = RGB565_BLUE;
    // 80%
    if(l > 192) col = RGB565_RED;
    tft.fillRect(288, 240 - l, 319, 239, col);
    
    sprintf(buf, "%5u", mn);
    tft.setCursor(0, 0);
    tft.writeString(buf);
    
    sprintf(buf, "%5u %.1lfnm", mx, nm, nm);
    tft.setCursor(200, 0);
    tft.writeString(buf);
}

// Display the sampled spectral
FAST_CODE_2 void plotSpectral(U16 * _ptr, U16 col, bool stats = true)
{
    U16     y, rng, mn, mx;
    static U16  _mn = 0, _mx = 0;
    
    if(stats)
    {
        stat.setData(otStatistics::T_U16, (void *) _ptr, otSpectrometerShield::SPEC_CHANNELS);
        stat.evaluate();
        rng = stat.max - stat.min;
        mn = stat.min;
        mx = stat.max;
        F64 nm = pixelToWavelength(stat.maxI);
        plotRange(mn, mx, nm);
        _mn = mn;
        _mx = mx;
    }
    else
    {
        mn = _mn;
        mx = _mx;
        rng = mx - mn;
    }
    
    // Force fixed range
    //rng = 25000;
    
    for(int i = 0; i < otSpectrometerShield::SPEC_CHANNELS; i++)
    {
        y = 240 - (_ptr[i] - mn) * 240 / rng;
        //tft.setPixel(i, y, RGB565_YELLOW);
        tft.drawLine(i, y, i, 239, col);
    }
}

void setup()
{
    UartEnableSignals(UART0);
    UartSetBaud(UART0, 115200);
    setStdout(4096, UART0); // printf output is now directed on UART0
    
    FpgaInit();
    FpgaHiddenPort(HP_EXT_I2C, true);
    
    I2cBusReset();
    I2cBusFrequency(400, 30);
    
    // Init graphics for spectral plot
    tft.init();
    tft.setRotation(otILI9341::IR_270);
    tft.clear();
    tft.setTextColor(RGB565_YELLOW, RGB565_BLACK);
    tft.setTextSize(1);
    // Init ADC
    adc.init();
    adc.setRange(otADS868x::UNI_5V);
    // Init spectrometer
    sp.init(&adc);
    
    // Init PGA
    pga.init();
    pga.gain(otPga::GAIN_1);
    // Set PGA channel
    pga.channel(CH_SPECTROMETER);
    // Allocate some buffers for this work!
    buffer = (U16 *) _malloc(otSpectrometerShield::SPEC_CHANNELS * sizeof(U16));
    _blank = (U16 *) _malloc(otSpectrometerShield::SPEC_CHANNELS * sizeof(U16));
    _filtered = (U16 *) _malloc(otSpectrometerShield::SPEC_CHANNELS * sizeof(U16));
}

void loop()
{
    bool    src = false;
    while(1)
    {
        // Alternate the two different light source
        if(src)
        {
            sp.setLightSource(otSpectrometerShield::SRC_LASER);
            src = false;
        }
        else
        {
            sp.setLightSource(otSpectrometerShield::SRC_LED);
            src = true;
        }
        // Grab 100 spectrals
        for(U16 i = 0; i < 100; i++)
        {
            // Set SPI ADC channel
            SpiSet(SA_ADC);
            // Save previous buffer
            _memcpy(_blank, _filtered, otSpectrometerShield::SPEC_CHANNELS * sizeof(U16));
            // Grab a new spectral
            sp.grabSpectral(buffer);
            // Smoothing remove spikes
            smoothing(buffer, _filtered);
            // Set LCD SPI channel
            SpiSet(SA_LCD);
            // Blank previous plot
            plotSpectral(_blank, RGB565_BLACK, false);
            // Plot latest spectral
            plotSpectral(_filtered, RGB565_YELLOW);
        }
    }
}