#include "usbnotify.h"
#include "usbdevice.h"
#include "StopWatch.h"
#include <iostream>
#include <string.h>

UsbNotify::UsbNotify(std::shared_ptr<UsbDevice> usb)
: brun(true)
{
    this->usb= usb;
    runThread.reset(new ThreadEx(&UsbNotify::run_notify, this));
}

UsbNotify::~UsbNotify()
{
    brun = false;
    ae.notify_all();
}

void UsbNotify::notify(void* data, int size)
{
    if(!usb || !usb->is_connected()) 
        return;

    std::lock_guard<std::mutex> lck(mx);
    // StopWatch sw;
    // while (msgs.empty() && sw.elapsed_ms() < 100);

    // if(!msgs.empty())
    //     msgs = std::queue<std::vector<unsigned char>>();

    std::vector<unsigned char> msg(size);
    memcpy(&msg[0],data,size);    
    msgs.push(msg);
    //msgs.push(std::vector<unsigned char>((unsigned char*)data, (unsigned char*)data + size));
    ae.notify_all();
}

void UsbNotify::clear()
{
    usb->abort_int();
    std::lock_guard<std::mutex> lck(mx);
    msgs = std::queue<std::vector<unsigned char>>();
}

void UsbNotify::run_notify()
{
    unsigned char buff[64]; 
    while(brun)
    {
        if (ae.wait(1000) && brun)
        {
            for(;;)
            {
                {
                    std::lock_guard<std::mutex> lck(mx);
                    if(msgs.empty())
                        break;
                    auto &data = msgs.front();
                    memset(buff,0,sizeof(buff));
                    std::copy(data.begin(), data.end(), buff);
                }

                if (usb && usb->is_connected())
                    usb->write_int(buff, sizeof(buff));

                {
                    //printf("\n notify msg ");
                    std::lock_guard<std::mutex> lck(mx);
                    if(msgs.size()>0)
                        msgs.pop();
                }
            }
        }
    }
}