#define DEBUG 1
/*
 Little app to demonstrate motion detection
*/
#include <features.h>
#include <stdio.h>
#include <getopt.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/types.h>
//#include <sys/time.h>
#include <asm/types.h>
#include <linux/ip.h>
#include <errno.h>
#include <sys/ioctl.h>

#include <X11/StringDefs.h>
#include <X11/Intrinsic.h>
#include <X11/Xaw/Porthole.h>

#include "xmotion.h"
//#include "videodev.h"


/* these globals contain the app default values */
int repeat_rate     = 1;
int capture_width   = 352;
int capture_height  = 240;
int debug           = 0;
static int vga_initted = 0;
int motion_threshold = 5;

#define TIME_SCALE 1  /* should be 1000 to -r option in seconds */

void snap(XtPointer p, XtIntervalId* t);


/* globals; I know, I know, these are not nice;
 but I have to pass them to the timer callback, so I should really
 pack them into a struct; is that the best way? */
int     h_vidcap;
bitmap* bm;
bitmap* bm_prev;
bitmap* bm_diff;
bitmap* bm_show;
int     pixel_depth;
XtAppContext app;
Widget    show_motion_wnd;
GC        imagegc;
XImage*   image;


/* wholly wrong; needs to be fixed */
void usage(void) {
    printf("xmotion: watches the video capture for motion\n"
           "when finished, will run a script based on various types of\n"
           "motion detected: darken, sweep-left-to-right, appear, disappear, etc\n"
           "syntax: xmotion [-h] [-r rate] [-w width] [-d] \n"
           "                [-b brighness] [-c contrast] etc, etc\n"
           "   -h help                 -d debug (ni)\n"
           "   -r rate in msecs         -w width in pixels\n"
           "   -b brightness[10-240] (ni)\n"
           "   -c contrast [30-90] (ni)\n"
           "   -m motion threshold     -p soundfile (ni)\n"
           " (ni) not implemented\n");
}
int main(int argc, char** argv)
{
    int i;
    int r; /* return values */
    Widget    root_wnd;
    
    while ((i = getopt(argc, argv, "dhr:w:q:m:")) != EOF) {
        switch (i) {
        case 'd':
            debug++;
            break;
        case 'r':
            repeat_rate = atoi(optarg);
            if (repeat_rate<0 || repeat_rate>10000) {
                printf("The -r parameter must be between 1 and 10000 seconds; \n"
                       "use 0 to force a single image\n");
                exit(1);
            }
            break;
        case 'w':
            capture_width = atoi(optarg);
            if (capture_width<10 || capture_width>748) {
                printf("The -w parameter must be between 10 and 748\n");
                exit(1);
            }
            capture_height = capture_width * _HEIGHT / _WIDTH;
            break;
        case 'h':
        default:
            usage();
            exit(1);
        }
        
    }

    /* open the vidcap; this could also have O_NONBLOCK, if you wish */
    h_vidcap = open("/dev/video0",O_RDONLY);
    if (h_vidcap<0) {
        printf("error: cannot open the video capture device\n");
        exit(1);
    }
#if DEBUG
    else
        printf("vidcap device open %d\n",h_vidcap);
#endif
    
    /* set up the capture session */
    struct video_format vf;
    vf.width = capture_width;
    vf.height = vf.width * 3 / 4;  /* this save the user from having to specify height */
    vf.depth = 24;
    vf.pixelformat = PIX_FMT_YUYV;
    vf.flags = 0;
    r = ioctl(h_vidcap, VIDIOC_S_FMT, &vf);
#if DEBUG
    printf("ioctl returned %d, width %d height %d errno %d\n",r,vf.width,vf.height,errno);
#endif
    
    /* just a quick read, which will tell us if we are in X
       the data is thrown away */
    r = ioctl(h_vidcap, VIDIOC_COREL_UNFREEZE, 0);
    if (r<0 && errno!=EAGAIN) {
        printf("error: this program must be executed from within a graphics environment\n"
               "such as Xwindows or svgalib (%d)\n",errno);
        exit(1);
    }

    // start X stuff: two windows, a GC and an XImage; realize them
    root_wnd = XtAppInitialize(&app, "xmotion", 0, 0,
                          &argc, argv, 0, 0, 0);
    show_motion_wnd =
        XtVaCreateManagedWidget("port",
                                portholeWidgetClass, root_wnd,
                                XtNwidth, vf.width,
                                XtNheight, vf.height,
                                0);
    XtRealizeWidget(root_wnd);

    pixel_depth = DefaultDepth(XtDisplay(show_motion_wnd),0);

    /* create a buffers to hold the bitmap */
    bm = new bitmap(vf.width, vf.height, 2);
    /* the previous one */
    bm_prev = new bitmap(vf.width, vf.height, 2);
    /* the one to display */
    bm_show = new bitmap(vf.width, vf.height, pixel_depth/8);
    /* the one to hold the diffs, for searching */
    bm_diff = new bitmap(vf.width, vf.height, 1);
    printf("mallocs done\n");

    
    imagegc = XCreateGC(XtDisplay(show_motion_wnd),
                        DefaultRootWindow(XtDisplay(show_motion_wnd)),
                        0, 0);
    image = XCreateImage(XtDisplay(show_motion_wnd),
                         DefaultVisual(XtDisplay(show_motion_wnd),0),
                         pixel_depth, ZPixmap,
                         0, (char*) bm_show->GetBits(),
                         vf.width,vf.height,
                         16,0);

    XtAppAddTimeOut(app, repeat_rate*TIME_SCALE, snap, 0);

    XtAppMainLoop(app);
}

inline int clip(int a) {
    if (a<0) return 0;
    if (a>255) return 255;
    return a;
}

void snap(XtPointer p, XtIntervalId* t)
{
    int r, i,j,acc;
    unsigned char* pBits = bm->GetBits();
    unsigned char* pOldBits = bm_prev->GetBits();
    unsigned char* pShowBits = bm_show->GetBits();
    unsigned char* pDiffBits = bm_diff->GetBits();

    /* loop reading until we get a full image */
    r = read(h_vidcap, bm->GetBits(), bm->Size());

    if (r<0) {
        printf("error reading capture file %d\n",errno);
        return;
    }

    acc = 0;
    int k;
	/* copy and convert the YUYV to RGB */
	for (i=0; i<bm->Height(); i++) {
	    for (k=0; k<bm->Width()/2; k++, pBits+=4) {
            int rp,gp,bp,y,u,v;
            y = pBits[0];
            u = pBits[1] - 128;
            v = pBits[3] - 128;
            rp = 219 * v / 160;
            gp = -(74 * u + 153 * v) / 219;
            bp = 219 * u / 126;

            switch (pixel_depth) {
            case 16:
                *(unsigned short*)pShowBits =
                    ((clip(y+rp) << 8) & 0xF800) |
                    ((clip(y+gp) << 3) & 0x07e0) |
                    ((clip(y+bp) >> 3) & 0x001f);
                y = pBits[2];
                *(unsigned short*)(pShowBits+2) =
                    ((clip(y+rp) << 8) & 0xF800) |
                    ((clip(y+gp) << 3) & 0x07e0) |
                    ((clip(y+bp) >> 3) & 0x001f);
                break;
            case 24:
                pShowBits[0] = clip(y + rp);
                pShowBits[1] = clip(y + gp);
                pShowBits[2] = clip(y + bp);
                y = pBits[2];
                pShowBits[3] = clip(y + rp);
                pShowBits[4] = clip(y + gp);
                pShowBits[5] = clip(y + bp);
                break;
            default:
                printf("not coded yet\n");
                exit(1);
                break;
            }
            pShowBits += 2*(pixel_depth/8);
        }
    }
      	
	pBits = bm->GetBits();
    /* find the moving areas */
    /* only compare the Y bytes, which are the even-number bytes */
    int xmin=bm->Width(),
        xmax=0,
        ymin=bm->Height(),
        ymax=0;
    for (i=0; i<bm->Height(); i++) {
        for (j=0; j<bm->Width(); j++, pBits+=2, pOldBits+=2, pDiffBits++) {
            *pDiffBits = abs(int(*pBits - *pOldBits));
//printf("diff is %d (%d %d)",*pDiffBits,*pBits,*pOldBits);
            if (*pDiffBits  > 45) {
                if (i>ymax) ymax = i;
                if (i<ymin) ymin = i;
                if (j>xmax) xmax = j;
                if (j<xmin) xmin = j;
            }
        }
    }
    pShowBits = bm_show->GetBits();
        if (xmax && ymax && xmax<bm->Width() && ymax<bm->Height()) {
        switch (pixel_depth) {
        case 16:
            for (j=xmin; j<xmax; j++) {
                ((unsigned short*)pShowBits)[ymin * bm->Width() + j] = 0xF800;
                ((unsigned short*)pShowBits)[ymax * bm->Width() + j] = 0xF800;
            }
            for (i=ymin; i<ymax; i++) {
                ((unsigned short*)pShowBits)[i * bm->Width() + xmin] = 0xF800;
                ((unsigned short*)pShowBits)[i * bm->Width() + xmax] = 0xF800;
            }
            /* crosshairs */
            for (j=(2*xmin + xmax)/3; j<(2*xmax+xmin)/3; j++)
                ((unsigned short*)pShowBits)[(ymin+ymax)/2 * bm->Width() + j] = 0xF800;
            for (i=(2*ymin + ymax)/3; i<(2*ymax+ymin)/3; i++)
                ((unsigned short*)pShowBits)[i * bm->Width() + (xmax + xmin)/2] = 0xF800;
            break;
        default:
            printf("not coded yet\n");
            exit(1);
            }
        }
        memcpy(bm_prev->GetBits(), bm->GetBits(), bm->Width() * bm->Height() * 2);

        if (XtIsRealized(show_motion_wnd))
        {
            XPutImage(XtDisplay(show_motion_wnd),
                      XtWindow(show_motion_wnd),
                      imagegc,
                      image,
                      0,0,0,0,
                      bm_show->Width(), bm_show->Height());
        }
        // restart the timer
        XtAppAddTimeOut(app, repeat_rate*TIME_SCALE, snap, 0);
//        printf("thresh %d, acc %d, nbits %d\n",motion_threshold, acc, bm->Size()/2);
}