#include "lasp_cppuldaq.h"
#include <chrono>
#include <iostream>
#include <cmath>
using std::cout;
using std::endl;


int main() {

    /* boolvec inChannels = {true, false, false, false}; */
    boolvec inChannels = {true, true, false, false};
    boolvec outChannels = {true};
    double samplerate = 10000;
    const us samplesPerBlock = 256;

    DT9837A daq(
            samplesPerBlock,
            inChannels,
            outChannels,
            samplerate,
            true // monitor Output
            );

    SafeQueue<void*> inqueue;
    SafeQueue<void*> outqueue;

    double totalTime = 5;
    double t = 0;
    double freq = 1000;
    us nblocks = ((us) totalTime*samplerate/samplesPerBlock) + 10;

    for(us i=0;i<nblocks;i++) {

        double* data = static_cast<double*>(malloc(sizeof(double)*samplesPerBlock));
        for(us sample=0;sample<samplesPerBlock;sample++) {

            data[sample] = sin(2*M_PI*freq*t);
            t+= 1.0/samplerate;

        }
        outqueue.enqueue(data);
    }

    daq.start(&inqueue, &outqueue);
    /* daq.start(NULL, &outqueue); */

    std::this_thread::sleep_for(std::chrono::seconds((int) totalTime));
    /* std::string a; */
    /* std::cin >> a; */

    daq.stop();

    while(!inqueue.empty()) {
        double* buf = (double*) inqueue.dequeue();
        for(us i=0;i<samplesPerBlock;i++) {
            for(us ch=0;ch<daq.neninchannels();ch++) {
                cout << buf[ch*samplesPerBlock+i] << " ";
            }
            cout << endl;
        }
        free(buf);
    }
    while(!outqueue.empty()){
        void* dat = outqueue.dequeue();
        free(dat);
    }

    return 0;
}