/*
 * 1394-Based Digital Camera Control Library
 *
 * Written by Damien Douxchamps <ddouxchamps@users.sf.net>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <unistd.h>
#include <signal.h>
#include <X11/Xlib.h>
#include <X11/Xutil.h>
#include <X11/extensions/Xvlib.h>
#include <X11/keysym.h>
#define _GNU_SOURCE
#include <getopt.h>
#include <stdint.h>
#include <inttypes.h>

#include "dc1394/dc1394.h"


/* uncomment the following to drop frames to prevent delays */
#define MAX_PORTS   4
#define MAX_CAMERAS 8
#define NUM_BUFFERS 8

/* ok the following constant should be by right included thru in Xvlib.h */
#ifndef XV_YV12
#define XV_YV12 0x32315659
#endif

#ifndef XV_YUY2
#define XV_YUY2 0x32595559
#endif

#ifndef XV_UYVY
#define XV_UYVY 0x59565955
#endif


/* declarations for libdc1394 */
uint32_t numCameras = 0;
dc1394camera_t *cameras[MAX_CAMERAS];
dc1394featureset_t features;
dc1394video_frame_t * frames[MAX_CAMERAS];

/* declarations for video1394 */
char *device_name=NULL;

/* declarations for Xwindows */
Display *display=NULL;
Window window=(Window)NULL;
unsigned long width,height;
unsigned long device_width,device_height;
int connection=-1;
XvImage *xv_image=NULL;
XvAdaptorInfo *info;
long format=0;
GC gc;


/* Other declarations */
unsigned long frame_length;
long frame_free;
int frame=0;
int adaptor=-1;

int freeze=0;
int average=0;
int fps;
int res;
char *frame_buffer=NULL;


static struct option long_options[]={
    {"device",1,NULL,0},
    {"fps",1,NULL,0},
    {"res",1,NULL,0},
    {"help",0,NULL,0},
    {NULL,0,0,0}
};

void get_options(int argc,char *argv[])
{
    int option_index=0;
    fps=7;
    res=0;

    while(getopt_long(argc,argv,"",long_options,&option_index)>=0){
        if(optarg){
            switch(option_index){
                /* case values must match long_options */
            case 0:
                device_name=strdup(optarg);
                break;
            case 1:
                fps=atoi(optarg);
                break;
            case 2:
                res=atoi(optarg);
                break;
            }
        }
        if(option_index==3){
            printf( "\n"
                    "        %s - multi-cam monitor for libdc1394 and XVideo\n\n"
                    "Usage:\n"
                    "        %s [--fps=[1,3,7,15,30]] [--res=[0,1,2]] [--device=/dev/video1394/x]\n"
                    "             --fps    - frames per second. default=7,\n"
                    "                        30 not compatible with --res=2\n"
                    "             --res    - resolution. 0 = 320x240 (default),\n"
                    "                        1 = 640x480 YUV4:1:1, 2 = 640x480 RGB8\n"
                    "             --device - specifies video1394 device to use (optional)\n"
                    "                        default = automatic\n"
                    "             --help   - prints this message\n\n"
                    "Keyboard Commands:\n"
                    "        q = quit\n"
                    "        < -or- , = scale -50%%\n"
                    "        > -or- . = scale +50%%\n"
                    "        0 = pause\n"
                    "        1 = set framerate to 1.875 fps\n"
                    "        2 = set framerate tp 3.75 fps\n"
                    "        3 = set framerate to 7.5 fps\n"
                    "        4 = set framerate to 15 fps\n"
                    "        5 = set framerate to 30 fps\n"
                    ,argv[0],argv[0]);
            exit(0);
        }
    }

}

/* image format conversion functions */

static inline
void iyu12yuy2 (unsigned char *src, unsigned char *dest, uint32_t NumPixels) {
    int i=0,j=0;
    register int y0, y1, y2, y3, u, v;
    while (i < NumPixels*3/2) {
        u = src[i++];
        y0 = src[i++];
        y1 = src[i++];
        v = src[i++];
        y2 = src[i++];
        y3 = src[i++];

        dest[j++] = y0;
        dest[j++] = u;
        dest[j++] = y1;
        dest[j++] = v;

        dest[j++] = y2;
        dest[j++] = u;
        dest[j++] = y3;
        dest[j++] = v;
    }
}


static inline
void rgb2yuy2 (unsigned char *RGB, unsigned char *YUV, uint32_t NumPixels) {
    int i, j;
    register int y0, y1, u0, u1, v0, v1 ;
    register int r, g, b;

    for (i = 0, j = 0; i < 3 * NumPixels; i += 6, j += 4) {
        r = RGB[i + 0];
        g = RGB[i + 1];
        b = RGB[i + 2];
        RGB2YUV (r, g, b, y0, u0 , v0);
        r = RGB[i + 3];
        g = RGB[i + 4];
        b = RGB[i + 5];
        RGB2YUV (r, g, b, y1, u1 , v1);
        YUV[j + 0] = y0;
        YUV[j + 1] = (u0+u1)/2;
        YUV[j + 2] = y1;
        YUV[j + 3] = (v0+v1)/2;
    }
}

/* helper functions */

void set_frame_length(unsigned long size, int numCameras)
{
    frame_length=size;
    dc1394_log_debug("Setting frame size to %ld kb",size/1024);
    frame_free=0;
    frame_buffer = malloc( size * numCameras);
}

void display_frames()
{
    uint32_t i;

    if(!freeze && adaptor>=0){
        for (i = 0; i < numCameras; i++) {
            if (!frames[i])
                continue;
            switch (res) {
            case DC1394_VIDEO_MODE_640x480_YUV411:
                iyu12yuy2( frames[i]->image,
                           (unsigned char *)(frame_buffer + (i * frame_length)),
                           (device_width*device_height) );
                break;

            case DC1394_VIDEO_MODE_320x240_YUV422:
            case DC1394_VIDEO_MODE_640x480_YUV422:
                memcpy( frame_buffer + (i * frame_length),
                        frames[i]->image, device_width*device_height*2);
                break;

            case DC1394_VIDEO_MODE_640x480_RGB8:
                rgb2yuy2( frames[i]->image,
                          (unsigned char *) (frame_buffer + (i * frame_length)),
                          (device_width*device_height) );
                break;
            }
        }

        xv_image=XvCreateImage(display,info[adaptor].base_id,format,frame_buffer,
                               device_width,device_height * numCameras);
        XvPutImage(display,info[adaptor].base_id,window,gc,xv_image,
                   0,0,device_width,device_height * numCameras,
                   0,0,width,height);

        xv_image=NULL;
    }
}

void QueryXv()
{
    uint32_t num_adaptors;
    int num_formats;
    XvImageFormatValues *formats=NULL;
    int i,j;
    char xv_name[5];

    XvQueryAdaptors(display,DefaultRootWindow(display),&num_adaptors,&info);

    for(i=0;i<num_adaptors;i++) {
        formats=XvListImageFormats(display,info[i].base_id,&num_formats);
        for(j=0;j<num_formats;j++) {
            xv_name[4]=0;
            memcpy(xv_name,&formats[j].id,4);
            if(formats[j].id==format) {
                dc1394_log_error("using Xv format 0x%x %s %s",formats[j].id,xv_name,(formats[j].format==XvPacked)?"packed":"planar");
                if(adaptor<0)adaptor=i;
            }
        }
    }
    XFree(formats);
    if(adaptor<0)
        dc1394_log_error("No suitable Xv adaptor found");

}


void cleanup(void) {
    int i;
    for (i=0; i < numCameras; i++) {
        dc1394_video_set_transmission(cameras[i], DC1394_OFF);
        dc1394_capture_stop(cameras[i]);
    }
    if ((void *)window != NULL)
        XUnmapWindow(display,window);
    if (display != NULL)
        XFlush(display);
    if (frame_buffer != NULL)
        free( frame_buffer );
}


/* trap ctrl-c */
void signal_handler( int sig) {
    signal( SIGINT, SIG_IGN);
    cleanup();
    exit(0);
}

int main(int argc,char *argv[])
{
    XEvent xev;
    XGCValues xgcv;
    long background=0x010203;
    int i, j;
    dc1394_t * d;
    dc1394camera_list_t * list;

    get_options(argc,argv);
    /* process options */
    switch(fps) {
    case 1: fps =        DC1394_FRAMERATE_1_875; break;
    case 3: fps =        DC1394_FRAMERATE_3_75; break;
    case 15: fps = DC1394_FRAMERATE_15; break;
    case 30: fps = DC1394_FRAMERATE_30; break;
    case 60: fps = DC1394_FRAMERATE_60; break;
    default: fps = DC1394_FRAMERATE_7_5; break;
    }
    switch(res) {
    case 1:
        res = DC1394_VIDEO_MODE_640x480_YUV411;
        device_width=640;
        device_height=480;
        format=XV_YUY2;
        break;
    case 2:
        res = DC1394_VIDEO_MODE_640x480_RGB8;
        device_width=640;
        device_height=480;
        format=XV_YUY2;
        break;
    default:
        res = DC1394_VIDEO_MODE_320x240_YUV422;
        device_width=320;
        device_height=240;
        format=XV_UYVY;
        break;
    }

    dc1394error_t err;

    d = dc1394_new ();
    if (!d)
        return 1;
    err=dc1394_camera_enumerate (d, &list);
    DC1394_ERR_RTN(err,"Failed to enumerate cameras");

    if (list->num == 0) {
        dc1394_log_error("No cameras found");
        return 1;
    }

    j = 0;
    for (i = 0; i < list->num; i++) {
        if (j >= MAX_CAMERAS)
            break;
        cameras[j] = dc1394_camera_new (d, list->ids[i].guid);
        if (!cameras[j]) {
            dc1394_log_warning("Failed to initialize camera with guid %llx", list->ids[i].guid);
            continue;
        }
        j++;
    }
    numCameras = j;
    dc1394_camera_free_list (list);

    if (numCameras == 0) {
        dc1394_log_error("No cameras found");
        exit (1);
    }

    /* setup cameras for capture */
    for (i = 0; i < numCameras; i++) {

        err=dc1394_video_set_iso_speed(cameras[i], DC1394_ISO_SPEED_400);
        DC1394_ERR_CLN_RTN(err,cleanup(),"Could not set ISO speed");

        err=dc1394_video_set_mode(cameras[i], res);
        DC1394_ERR_CLN_RTN(err,cleanup(),"Could not set video mode");

        err=dc1394_video_set_framerate(cameras[i], fps);
        DC1394_ERR_CLN_RTN(err,cleanup(),"Could not set framerate");

        err=dc1394_capture_setup(cameras[i],NUM_BUFFERS, DC1394_CAPTURE_FLAGS_DEFAULT);
        DC1394_ERR_CLN_RTN(err,cleanup(),"Could not setup camera-\nmake sure that the video mode and framerate are\nsupported by your camera");

        err=dc1394_video_set_transmission(cameras[i], DC1394_ON);
        DC1394_ERR_CLN_RTN(err,cleanup(),"Could not start camera iso transmission");

    }

    fflush(stdout);
    if (numCameras < 1) {
        perror("no cameras found :(\n");
        cleanup();
        exit(-1);
    }

    switch(format){
    case XV_YV12:
        set_frame_length(device_width*device_height*3/2, numCameras);
        break;
    case XV_YUY2:
    case XV_UYVY:
        set_frame_length(device_width*device_height*2, numCameras);
        break;
    default:
        dc1394_log_error("Unknown format set (internal error)");
        exit(255);
    }

    /* make the window */
    display=XOpenDisplay(getenv("DISPLAY"));
    if(display==NULL) {
        dc1394_log_error("Could not open display \"%s\"",getenv("DISPLAY"));
        cleanup();
        exit(-1);
    }

    QueryXv();

    if ( adaptor < 0 ) {
        cleanup();
        exit(-1);
    }

    width=device_width;
    height=device_height * numCameras;

    window=XCreateSimpleWindow(display,DefaultRootWindow(display),0,0,width,height,0,
                               WhitePixel(display,DefaultScreen(display)),
                               background);

    XSelectInput(display,window,StructureNotifyMask|KeyPressMask);
    XMapWindow(display,window);
    connection=ConnectionNumber(display);

    gc=XCreateGC(display,window,0,&xgcv);

    /* main event loop */
    while(1){

        for (i = 0; i < numCameras; i++) {
            if (dc1394_capture_dequeue(cameras[i], DC1394_CAPTURE_POLICY_WAIT, &frames[i])!=DC1394_SUCCESS)
                dc1394_log_error("Failed to capture from camera %d", i);
        }

        display_frames();
        XFlush(display);

        while(XPending(display)>0){
            XNextEvent(display,&xev);
            switch(xev.type){
            case ConfigureNotify:
                width=xev.xconfigure.width;
                height=xev.xconfigure.height;
                display_frames();
                break;
            case KeyPress:
                switch(XKeycodeToKeysym(display,xev.xkey.keycode,0)){
                case XK_q:
                case XK_Q:
                    cleanup();
                    exit(0);
                    break;
                case XK_comma:
                case XK_less:
                    width=width/2;
                    height=height/2;
                    XResizeWindow(display,window,width,height);
                    display_frames();
                    break;
                case XK_period:
                case XK_greater:
                    width=2*width;
                    height=2*height;
                    XResizeWindow(display,window,width,height);
                    display_frames();
                    break;
                case XK_0:
                    freeze = !freeze;
                    break;
                case XK_1:
                    fps =        DC1394_FRAMERATE_1_875;
                    for (i = 0; i < numCameras; i++)
                        dc1394_video_set_framerate(cameras[i], fps);
                    break;
                case XK_2:
                    fps =        DC1394_FRAMERATE_3_75;
                    for (i = 0; i < numCameras; i++)
                        dc1394_video_set_framerate(cameras[i], fps);
                    break;
                case XK_4:
                    fps = DC1394_FRAMERATE_15;
                    for (i = 0; i < numCameras; i++)
                        dc1394_video_set_framerate(cameras[i], fps);
                    break;
                case XK_5:
                    fps = DC1394_FRAMERATE_30;
                    for (i = 0; i < numCameras; i++)
                        dc1394_video_set_framerate(cameras[i], fps);
                    break;
                case XK_3:
                    fps = DC1394_FRAMERATE_7_5;
                    for (i = 0; i < numCameras; i++)
                        dc1394_video_set_framerate(cameras[i], fps);
                    break;
                }
                break;
            }
        } /* XPending */

        for (i = 0; i < numCameras; i++) {
            if (frames[i])
                dc1394_capture_enqueue (cameras[i], frames[i]);
        }

    } /* while not interrupted */

    exit(0);
}