
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <getopt.h>
#include <linux/videodev2.h>
#include <linux/fb.h>
#include <sys/ioctl.h>
#include <sys/mman.h>

#include <SDL2/SDL.h>

#include <jpeglib.h>
#include <setjmp.h>

#define NB_BUFFER 2	// 如果内存较小，较小该数值, 默认４

struct videoDev {
	int fd;
	char devName[64];
	struct v4l2_capability cap;
	struct v4l2_fmtdesc fmtdesc;
	struct v4l2_format fmt;
	struct v4l2_buffer buf;
	struct v4l2_requestbuffers rb;
	void *mem[NB_BUFFER];
	unsigned char *rawbuf;
	int raw_w;
	int raw_h;
	int raw_format;
	int raw_size;
	unsigned char *rgbbuf;
	int rgb_w;
	int rgb_h;
	int rgb_size;
	int isstreaming;
};

struct videoDev vdev;


//================================================
// 打开视频流
//================================================
static int play_stream(struct videoDev *vd)
{
	int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	int ret;

	ret = ioctl(vd->fd, VIDIOC_STREAMON, &type);
	if (ret < 0) {
		printf("Error: Unable to start capture\n");
		return ret;
	}
	vd->isstreaming = 1;
	return 0;
}

//================================================
// 关闭视频流
//================================================
static int stop_stream(struct videoDev *vd)
{
	int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	int ret;

	ret = ioctl(vd->fd, VIDIOC_STREAMOFF, &type);
	if (ret < 0) {
		printf("Error: Unable to stop capture\n");
		return ret;
	}
	vd->isstreaming = 0;
	return 0;
}

//================================================
// 打开视频设备
//================================================
static int open_video_device(struct videoDev *vd)
{
	int i, ret = 0;
	
	// open video device
	if ((vd->fd = open(vd->devName, O_RDWR)) == -1) {
		printf("Error opening V4L interface\n");
		return -1;
	}
	
	// query capability
	memset(&vd->cap, 0, sizeof(struct v4l2_capability));
	ret = ioctl(vd->fd, VIDIOC_QUERYCAP, &vd->cap);
	if (ret < 0) {
		printf("Error: Unable to query device capability\n");
		return -1;
	}
	printf("cap.driver..: %s\n", vd->cap.driver);
	printf("cap.card....: %s\n", vd->cap.card);
	printf("cap.bus_info: %s\n", vd->cap.bus_info);
	printf("cap.version.: %u.%u.%u\n", (vd->cap.version>>16)&0xff, (vd->cap.version>>8)&0xff, vd->cap.version&0xff);

	if ((vd->cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
		printf("Error: video capture not supported\n");
		return -1;
	}

	// query format
	vd->fmtdesc.index = 0;
	vd->fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	printf("Support Formats:\n");
	while (ioctl(vd->fd, VIDIOC_ENUM_FMT, &vd->fmtdesc) != -1) {
		printf("%d. %s\n", vd->fmtdesc.index+1, vd->fmtdesc.description);
		vd->fmtdesc.index++;
	}
	
	// set format
	memset(&vd->fmt, 0, sizeof(struct v4l2_format));
	vd->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	vd->fmt.fmt.pix.width = vd->raw_w;
	vd->fmt.fmt.pix.height = vd->raw_h;
	vd->fmt.fmt.pix.pixelformat = vd->raw_format;
	vd->fmt.fmt.pix.field = V4L2_FIELD_ANY;
	ret = ioctl(vd->fd, VIDIOC_S_FMT, &vd->fmt);
	if (ret < 0) {
		printf("Error: Unable to set format\n");
		return -1;
	}
	
	// request buffers
	memset(&vd->rb, 0, sizeof(struct v4l2_requestbuffers));
	vd->rb.count = NB_BUFFER;
	vd->rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	vd->rb.memory = V4L2_MEMORY_MMAP;
	ret = ioctl(vd->fd, VIDIOC_REQBUFS, &vd->rb);
	if (ret < 0) {
		printf("Error: Unable to allocate buffers\n");
		return -1;	
	}
	
	// map the buffers
	for (i=0; i<NB_BUFFER; i++) {
		memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
		vd->buf.index = i;
		vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		vd->buf.memory = V4L2_MEMORY_MMAP;
		ret = ioctl(vd->fd, VIDIOC_QUERYBUF, &vd->buf);
		if (ret < 0) {
			printf("Error: Unable to query buffer\n");
			return -1;
		}
		
		vd->mem[i] = mmap(0, vd->buf.length, PROT_READ, MAP_SHARED, vd->fd, vd->buf.m.offset);
		if (vd->mem[i] == MAP_FAILED) {
			printf("Error: Unable to map buffer\n");
			return -1;	
		}
	}

	// queue the buffers
	for (i=0; i<NB_BUFFER; i++) {
		memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
		vd->buf.index = i;
		vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		vd->buf.memory = V4L2_MEMORY_MMAP;
		ret = ioctl(vd->fd, VIDIOC_QBUF, &vd->buf);
		if (ret < 0) {
			printf("Error: Unable to query buffer\n");
			return -1;
		}
	}
	return 0;
}

//================================================
// 关闭视频设备
//================================================
static int close_video_device(struct videoDev *vd)
{
	if (vd->isstreaming) 
		stop_stream(vd);
	close(vd->fd);
	return 0;
}

//================================================
// 获取视频帧
//================================================
static int grab_frame(struct videoDev *vd)
{
	#define HEADERFRAME1 0xaf
	int ret;

	if (!vd->isstreaming)
		return -1;
	
	memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
	vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	vd->buf.memory = V4L2_MEMORY_MMAP;
	
	ret = ioctl(vd->fd, VIDIOC_DQBUF, &vd->buf);
	if (ret < 0) {
		printf("Error: Unable to dequery buffer\n");
		return -1;
	}

	switch (vd->raw_format) {
	case V4L2_PIX_FMT_MJPEG:
		if (vd->buf.bytesused == HEADERFRAME1) {
			printf("Ignoring empty buffer...\n");
			return -1;
		}
		
		memcpy(vd->rawbuf, vd->mem[vd->buf.index], vd->buf.bytesused);
		vd->raw_size = vd->buf.bytesused;
		break;
	}
	
	ret = ioctl(vd->fd, VIDIOC_QBUF, &vd->buf);
	if (ret < 0) {
		printf("Error: Unable to requery buffer\n");
		return -1;
	}

	return 0;
}



//================================================
// JPEG解码
//================================================
struct my_error_mgr {
	struct jpeg_error_mgr pub;
	jmp_buf setjmp_buffer;
};
typedef struct my_error_mgr *my_error_ptr;

METHODDEF(void) my_error_exit(j_common_ptr cinfo)
{
	my_error_ptr myerr = (my_error_ptr) cinfo->err;
	(*cinfo->err->output_message)(cinfo);
	longjmp(myerr->setjmp_buffer, 1);
}

int jpegDecode(unsigned char *pjpg, int jpglen, unsigned char *prgb)
{
	struct jpeg_decompress_struct cinfo;
	struct my_error_mgr jerr;
	
	cinfo.err = jpeg_std_error(&jerr.pub);
	jerr.pub.error_exit = my_error_exit;
	if (setjmp(jerr.setjmp_buffer)) {
		jpeg_destroy_decompress(&cinfo);
		return -1;
	}

	jpeg_create_decompress(&cinfo);
	jpeg_mem_src(&cinfo, pjpg, jpglen);
	jpeg_read_header(&cinfo, TRUE);
	jpeg_start_decompress(&cinfo);
	
	int row_stride = cinfo.output_width * cinfo.output_components;
	
	JSAMPARRAY buffer = (*cinfo.mem->alloc_sarray)((j_common_ptr)&cinfo, JPOOL_IMAGE, row_stride, 1);

	unsigned char *p = prgb;
	int bytes = 0;
	
	while (cinfo.output_scanline < cinfo.output_height) {
		jpeg_read_scanlines(&cinfo, buffer, 1);

		memcpy(prgb+bytes, buffer[0], row_stride);
		bytes += row_stride;
	}

	jpeg_finish_decompress(&cinfo);
	jpeg_destroy_decompress(&cinfo);
	return 0;	
	
}


SDL_Window *g_window = NULL;
SDL_Renderer *g_render = NULL;
SDL_Texture *g_texture = NULL;

//================================================
// SDL2释放
//================================================
void sdl_free()
{
    if (g_texture!=NULL) {
        SDL_DestroyTexture(g_texture);
        g_texture = NULL;
    }
    if (g_render!=NULL) {
        SDL_DestroyRenderer(g_render);
        g_render = NULL;
    }
    if (g_window!=NULL) {
        SDL_DestroyWindow(g_window);
        g_window = NULL;
    }
    SDL_Quit();
}

//================================================
// SDL2初始化
//================================================
int sdl_init(int videow, int videoh)
{
    if (SDL_Init(SDL_INIT_VIDEO) < 0) {
        SDL_LogError(SDL_LOG_CATEGORY_APPLICATION, "Couldn't initialize SDL: %s", SDL_GetError());
        goto sdlErr;
    }

    g_window = SDL_CreateWindow("display", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED,
                                640, 480, SDL_WINDOW_SHOWN|SDL_WINDOW_OPENGL);
                        
    if (!g_window) {
        SDL_LogError(SDL_LOG_CATEGORY_APPLICATION, "Couldn't create SDL window: %s", SDL_GetError());
        goto sdlErr;
    }

    g_render = SDL_CreateRenderer(g_window, -1, 0);
    if (!g_render) {
        SDL_LogError(SDL_LOG_CATEGORY_APPLICATION, "Couldn't create SDL renderer: %s", SDL_GetError());
        goto sdlErr;
    }

    g_texture = SDL_CreateTexture(g_render,
                                  SDL_PIXELFORMAT_RGB24,
                                  SDL_TEXTUREACCESS_STREAMING,
                                  videow, videoh);
    if (!g_texture) {
        SDL_LogError(SDL_LOG_CATEGORY_APPLICATION, "Couldn't create SDL texture: %s", SDL_GetError());
        goto sdlErr;
    }

    return true;

 sdlErr:
    sdl_free();
    return false;
}



//================================================
// SDL2显示
//================================================
void sdl_display(unsigned char *buffer, int width, int height)
{
    SDL_UpdateTexture(g_texture, NULL, buffer, width*3);
    
	SDL_Rect srcrect;
	srcrect.x = 0;
	srcrect.y = 0;
	srcrect.w = width;
	srcrect.h = height;
	SDL_Rect dstrect;
	dstrect.x = 0;
	dstrect.y = 0;
	dstrect.w = 640;
	dstrect.h = 480;

    SDL_RenderClear(g_render);
    SDL_RenderCopy(g_render, g_texture, &srcrect, &dstrect);
    SDL_RenderPresent(g_render);
}

//================================================
// 主函数
//================================================
int main(int argc, char *argv[])
{
	strcpy(vdev.devName, "/dev/video0");
	vdev.raw_w = 4160;
	vdev.raw_h = 3120;
	vdev.raw_format = V4L2_PIX_FMT_MJPEG;
	vdev.isstreaming = 0;

	if (open_video_device(&vdev) < 0) 
		return -1;
	if (play_stream(&vdev) < 0)
		return -1;
	
	if (sdl_init(vdev.raw_w,vdev.raw_h) < 0)
		return -1;

	vdev.rawbuf = (unsigned char*)calloc(1, vdev.raw_w*vdev.raw_h*2);
	vdev.rgb_w = vdev.raw_w;
	vdev.rgb_h = vdev.raw_h;
	vdev.rgb_size = vdev.rgb_w*vdev.rgb_h*4;
	vdev.rgbbuf = (unsigned char*)calloc(1, vdev.rgb_size);
	
	
	while (1) 
	{
		SDL_Event event;
		while (SDL_PollEvent(&event)) {
		    if (event.type == SDL_QUIT) {
		        goto ExitApp;
		    }
		}

		if (grab_frame(&vdev) < 0) {
			sleep(10);
			continue;
		}
		
		jpegDecode(vdev.rawbuf, vdev.raw_size, vdev.rgbbuf);

		sdl_display(vdev.rgbbuf, vdev.rgb_w, vdev.rgb_h);
	}
	
ExitApp:

	sdl_free();

	stop_stream(&vdev);
	close_video_device(&vdev);
	free(vdev.rawbuf);
	free(vdev.rgbbuf);
	return 0;
}
