#include "UVCDevice.h"
#include <iostream>

void UVCDevice::init(int index, uvc_context_t *ctx, uvc_device_t *dev)
{
    uvc_error_t ret;
    uvc_device_descriptor_t *des;
    this->index = index;
    ret = uvc_get_device_descriptor(dev, &des);
    if (ret < 0)
    {
        uvc_perror(ret, "uvc_find_device");
        return;
    }

    ret = uvc_open(dev, &devh);

    if (ret < 0)
    {
        uvc_perror(ret, "uvc_open");
        return;
    }

    std::cout << "Device Find Handle is " << devh << std::endl;

    uvc_free_device_descriptor(des);

    // Print out a message containing all the information that libuvc knows about the device
    // uvc_print_diag(devh, stderr);

    const uvc_format_desc_t *format_desc = uvc_get_format_descs(devh);
    const uvc_frame_desc_t *frame_desc = format_desc->frame_descs;

    // set default frame_format
    switch (format_desc->bDescriptorSubtype)
    {
    case UVC_VS_FORMAT_MJPEG:
        frame_format = UVC_COLOR_FORMAT_MJPEG;
        break;
    case UVC_VS_FORMAT_FRAME_BASED:
        frame_format = UVC_FRAME_FORMAT_H264;
        break;
    default:
        frame_format = UVC_FRAME_FORMAT_YUYV;
        break;
    }
}

void UVCDevice::setStreamParameters(int width, int height, int fps, uvc_frame_format frame_format)
{
    this->width = width;
    this->height = height;
    this->fps = fps;
    this->frame_format = frame_format;
}

void UVCDevice::setCallBack(uvc_frame_callback_t *cb)
{
    this->cb = cb;
}

void UVCDevice::startStreaming()
{
    uvc_error_t res;

    uvc_device_t *dev_device = uvc_get_device(devh);

    res = uvc_get_stream_ctrl_format_size(devh, &ctrl, frame_format, width, height, fps);
    if (res < 0)
    {
        uvc_perror(res, "get_mode");
    }

    /* Print out the result */
    uvc_print_stream_ctrl(&ctrl, stderr);

    res = uvc_start_streaming(devh, &ctrl, cb, &index, 0);
    if (res < 0)
    {
        uvc_perror(res, "start_streaming");
    }

    puts("Streaming...");
    /* enable auto exposure - see uvc_set_ae_mode documentation */
    puts("Enabling auto exposure ...");
    const uint8_t UVC_AUTO_EXPOSURE_MODE_AUTO = 4;
    res = uvc_set_ae_mode(devh, UVC_AUTO_EXPOSURE_MODE_AUTO);
    if (res == UVC_SUCCESS)
    {
        // puts(" ... enabled auto exposure");
        //set exposure time
        uint32_t exposure_time = 250; // 1 second in microseconds
        res = uvc_set_exposure_abs(devh, exposure_time);
        // res = uvc_set_white_balance_component_auto(devh,0);
        if (res < 0)
        {
            uvc_perror(res, " ... uvc_set_exposure_abs failed");
        }
        else
        {
            puts(" ... exposure time set to 1 second");
        }

    }
    else if (res == UVC_ERROR_PIPE)
    {
        /* this error indicates that the camera does not support the full AE mode;
         * try again, using aperture priority mode (fixed aperture, variable exposure time) */
        puts(" ... full AE not supported, trying aperture priority mode");
        const uint8_t UVC_AUTO_EXPOSURE_MODE_APERTURE_PRIORITY = 8;
        res = uvc_set_ae_mode(devh, UVC_AUTO_EXPOSURE_MODE_APERTURE_PRIORITY);
        if (res < 0)
        {
            uvc_perror(res, " ... uvc_set_ae_mode failed to enable aperture priority mode");
        }
        else
        {
            puts(" ... enabled aperture priority auto exposure mode");
        }
    }
    else
    {
        uvc_perror(res, " ... uvc_set_ae_mode failed to enable auto exposure mode");
    }
}

void UVCDevice::stopStreaming()
{
    uvc_stop_streaming(devh);
    puts("Done streaming.");
}

void UVCDevice::close()
{
    uvc_unref_device(dev);
}