#include "usb_cam_python.h"

#include <vector>
#include <iostream>

#include <thread>
#include <mutex>
#include <condition_variable>


namespace iv {

struct threadcam
{
    std::thread * mpthread;
    std::string mstrvideoname;
    std::shared_ptr<char> mpstr_data;
    int mndatasize;
    std::mutex mmutexdata;
    usb_cam::UsbCam * mpusbcam;
    bool mbRun;
    bool mbUpdate;
    std::condition_variable mcv;
    std::mutex mmutexcv;
};


}

std::vector<iv::threadcam * >  gvectorthreadcam;

std::mutex gmutex;



using namespace  usb_cam;

bool gstreaming_status_;
int gimage_width_, gimage_height_, gframerate_, gexposure_, gbrightness_, gcontrast_, gsaturation_, gsharpness_, gfocus_,
    gwhite_balance_, ggain_;
bool gautofocus_, gautoexposure_, gauto_white_balance_;



void setdefaultcvalue()
{
    gbrightness_ = -1;// xp.GetParam("brightness",  -1); //0-255, -1 "leave alone"
    gcontrast_ = -1;//xp.GetParam("contrast",  -1); //0-255, -1 "leave alone"
    gsaturation_ =  -1;//xp.GetParam("saturation",  -1); //0-255, -1 "leave alone"
    gsharpness_ = -1;//xp.GetParam("sharpness",  -1); //0-255, -1 "leave alone"
    // possible values: mmap, read, userptr
//    gio_method_name_ = xp.GetParam("io_method",  std::string("mmap"));
//    gimage_width_ = xp.GetParam("image_width", 1920);
//    gimage_height_ = xp.GetParam("image_height",  1080);
//    gframerate_ =  xp.GetParam("framerate",  30);
    // possible values: yuyv, uyvy, mjpeg, yuvmono10, rgb24
//    gpixel_format_name_ = xp.GetParam("pixel_format",  std::string("mjpeg"));
    // enable/disable autofocus
    gautofocus_  = false;//xp.GetParam("autofocus", false);


    gfocus_ = -1;//xp.GetParam("focus", -1); //0-255, -1 "leave alone"
    // enable/disable autoexposure
    gautoexposure_ = true;//xp.GetParam("autoexposure",  true);
    gexposure_ = 100;//xp.GetParam("exposure",  100);
    ggain_ = -1;//xp.GetParam("gain",  -1); //0-100?, -1 "leave alone"
    // enable/disable auto white balance temperature
    gauto_white_balance_ = true;//xp.GetParam("auto_white_balance", true);
    gwhite_balance_ = 4000;//xp.GetParam("white_balance", 4000);

    // load the camera info
//     xp.GetParam("camera_frame_id", img_.header.frame_id, std::string("head_camera"));
//    gcamera_name_ = "head_camera";// xp.GetParam("camera_name",  std::string("head_camera"));
//    gcamera_info_url_ = "";//xp.GetParam("camera_info_url",  std::string(""));
}


void threadcam(iv::threadcam * pthreadcam)
{

    while(pthreadcam->mbRun)
    {
        std::shared_ptr<char> mpstr_buf = std::shared_ptr<char>(new char[1000000]);
        int nLen = 0;
  //      std::cout<< "grab."<<std::endl;
        pthreadcam->mpusbcam->grab_image(mpstr_buf.get(),&nLen,1000000);
        if(nLen> 0)
        {
            pthreadcam->mmutexdata.lock();
            pthreadcam->mndatasize = nLen;
            pthreadcam->mpstr_data = mpstr_buf;
            pthreadcam->mbUpdate = true;
            pthreadcam->mmutexdata.unlock();
            pthreadcam->mcv.notify_all();
 //           std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000<< " "<<nLen<<std::endl;
        }
        else
        {
            std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" read camera "<<pthreadcam->mstrvideoname<<" fail."<<std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }

    pthreadcam->mpusbcam->stop_capturing();
    pthreadcam->mpusbcam->shutdown();
    delete pthreadcam->mpusbcam;


}


extern "C"
{

int StartCam(const char * strvideoname,int image_width,int image_height,int image_framerate)
{

    usb_cam::UsbCam * pcamx = new usb_cam::UsbCam;

    // set the IO method
    UsbCam::io_method io_method = UsbCam::io_method_from_string("mmap");
    if(io_method == UsbCam::IO_METHOD_UNKNOWN)
    {
        std::cout<<"Unknown IO method mmap "<<std::endl;
      return 0;
    }



    // set the pixel format
    UsbCam::pixel_format pixel_format = UsbCam::pixel_format_from_string("mjpeg");
    if (pixel_format == UsbCam::PIXEL_FORMAT_UNKNOWN)
    {
        std::cout<<" Unknown pixel format mjpeg"<<std::endl;
      return 0;
    }

//    std::cout<<"hear."<<std::endl;

    std::cout<<"video name "<<strvideoname<<std::endl;



    // start the camera
    pcamx->start(strvideoname, io_method, pixel_format, image_width,
             image_height, image_framerate);


    setdefaultcvalue();




    // set camera parameters
    if (gbrightness_ >= 0)
    {
      pcamx->set_v4l_parameter("brightness", gbrightness_);
    }

    if (gcontrast_ >= 0)
    {
      pcamx->set_v4l_parameter("contrast", gcontrast_);
    }

    if (gsaturation_ >= 0)
    {
      pcamx->set_v4l_parameter("saturation", gsaturation_);
    }

    if (gsharpness_ >= 0)
    {
      pcamx->set_v4l_parameter("sharpness", gsharpness_);
    }

    if (ggain_ >= 0)
    {
      pcamx->set_v4l_parameter("gain", ggain_);
    }

    // check auto white balance
    if (gauto_white_balance_)
    {
      pcamx->set_v4l_parameter("white_balance_temperature_auto", 1);
    }
    else
    {
      pcamx->set_v4l_parameter("white_balance_temperature_auto", 0);
      pcamx->set_v4l_parameter("white_balance_temperature", gwhite_balance_);
    }

    // check auto exposure
    if (!gautoexposure_)
    {
      // turn down exposure control (from max of 3)
      pcamx->set_v4l_parameter("exposure_auto", 1);
      // change the exposure level
      pcamx->set_v4l_parameter("exposure_absolute", gexposure_);
    }

    // check auto focus
    if (gautofocus_)
    {
      pcamx->set_auto_focus(1);
      pcamx->set_v4l_parameter("focus_auto", 1);
    }
    else
    {
      pcamx->set_v4l_parameter("focus_auto", 0);
      if (gfocus_ >= 0)
      {
        pcamx->set_v4l_parameter("focus_absolute", gfocus_);
      }
    }



    pcamx->set_useRawMJPEG(true);

    iv::threadcam  * xthreadcam = new iv::threadcam;
    xthreadcam->mbRun = true;
    xthreadcam->mbUpdate = false;
    xthreadcam->mpusbcam = pcamx;
    xthreadcam->mstrvideoname = strvideoname;


    xthreadcam->mpthread = new std::thread(threadcam,xthreadcam);

    gvectorthreadcam.push_back(xthreadcam);
    return 1;
}


int GetJPEGDataWithWait(char * strvideoname, char * str,int * x,int nwaitms)
{
    std::string strvideo = strvideoname;

    int nsize = gvectorthreadcam.size();
    int i;
    iv::threadcam * pthreadcam = NULL;
    for(i=0;i<nsize;i++)
    {
        if(gvectorthreadcam[i]->mstrvideoname == strvideo)
        {
            pthreadcam = gvectorthreadcam[i];
            break;
        }
    }

    if(pthreadcam == NULL)
    {
        std::cout<<" video device not open."<<std::endl;
        return 0;
    }

    int nRtn = 0;
    pthreadcam->mmutexdata.lock();
    if(pthreadcam->mbUpdate)
    {
//        str.copy(gstrbuf,gnLen);
        memcpy(str,pthreadcam->mpstr_data.get(),pthreadcam->mndatasize);
        nRtn = pthreadcam->mndatasize;
        *x = pthreadcam->mndatasize;
        pthreadcam->mbUpdate= false;
    }
    pthreadcam->mmutexdata.unlock();

    if(nRtn > 0)return nRtn;

    if(nwaitms == 0)return nRtn;

    std::unique_lock<std::mutex> lk(pthreadcam->mmutexcv);
    if(pthreadcam->mcv.wait_for(lk,std::chrono::milliseconds(nwaitms)) == std::cv_status::timeout)
    {
        lk.unlock();
    }
    else
    {
        lk.unlock();
    }
    pthreadcam->mmutexdata.lock();
    if(pthreadcam->mbUpdate)
    {
//        str.copy(gstrbuf,gnLen);
        memcpy(str,pthreadcam->mpstr_data.get(),pthreadcam->mndatasize);
        nRtn = pthreadcam->mndatasize;
        *x = pthreadcam->mndatasize;
        pthreadcam->mbUpdate= false;
    }
    pthreadcam->mmutexdata.unlock();

    return nRtn;


}

int GetJPEGData(char * strvideoname, char * str,int * x)
{
    return GetJPEGDataWithWait(strvideoname,str,x,0);
}

int StopCam(char * strvideoname)
{
    std::string strvideo = strvideoname;

    int nsize =gvectorthreadcam.size();
    int i;
    iv::threadcam * pthreadcam = NULL;
    int index;
    gmutex.lock();
    for(i=0;i<nsize;i++)
    {
        if(gvectorthreadcam[i]->mstrvideoname == strvideo)
        {
            pthreadcam = gvectorthreadcam[i];
            index = i;
            break;
        }
    }

    if(pthreadcam == NULL)
    {
        std::cout<<" video device not open."<<std::endl;
        gmutex.unlock();

        return 0;
    }

    pthreadcam->mbRun = false;
    pthreadcam->mpthread->join();
    gvectorthreadcam.erase(gvectorthreadcam.begin() + index);

    gmutex.unlock();


}



}