#include <OpenCV/OpenCV.h>

const char  * WINDOW_NAME  = "Filters";

int main (int argc, char * const argv[]) 
{
    // create all necessary instances
    cvNamedWindow (WINDOW_NAME, CV_WINDOW_AUTOSIZE);
    CvCapture * camera = cvCreateCameraCapture (CV_CAP_ANY);
    
    // you do own an iSight, don't you ?!?
    if (! camera)
        abort ();

    // get an initial frame and duplicate it for later work
    IplImage *  current_frame = cvQueryFrame (camera);
    IplImage *  src_image    = cvCreateImage(cvSize (current_frame->width, current_frame->height), IPL_DEPTH_8U, 3);
    IplImage *  quad_image    = cvCreateImage(cvSize (current_frame->width, current_frame->height), IPL_DEPTH_8U, 3);
    IplImage *  draw_image    = cvCreateImage(cvSize (current_frame->width, current_frame->height), IPL_DEPTH_8U, 3);
    assert (current_frame && /*gray_image &&*/ draw_image);
        
    //init
    bool quadImages = false;
    
    int height = draw_image->height;
    int width = draw_image->width;
    int arrayWidth = width*3;
    int arrayWidth2 = arrayWidth/2;
    //int size = height*width*3;
    int height2 = height/2;
    
    //filter gauss
    double filter0[] = {
        0, 0, 0,
        0, 1.0, 0,
        0, 0, 0
    };
    
    double filter1[] = {
        -1.0, -2.0, -1.0,
        0, 0, 0,
        1.0, 2.0, 1.0
    };
    
    double filter2[] = {
        -1.0, 0, 1.0,
        -2.0, 0, 2.0,
        -1.0, 0, 1.0
    };
    
    double filter3[] = {
        0, -1.0, 0.0,
        -1.0, 4.0, -1.0,
        0.0, -1.0, 0.0
    };
    
    float filter4d = 9.0;
    double filter4[] = {
        1.0/filter4d, 1.0/filter4d, 1.0/filter4d,
        1.0/filter4d, 1.0/filter4d, 1.0/filter4d,
        1.0/filter4d, 1.0/filter4d, 1.0/filter4d
    };
    
    double filter5[] = {
        1.0/16.0, 2.0/16.0, 1.0/16.0,
        2.0/16.0, 4.0/16.0, 2.0/16.0,
        1.0/16.0, 2.0/16.0, 1.0/16.0
    };
    
    float filter6d = 256.0;
    double filter6[] = {
        1.0/filter6d, 4.0/filter6d, 6.0/filter6d, 4.0/filter6d, 1.0/filter6d,
        4.0/filter6d, 16.0/filter6d, 24.0/filter6d, 16.0/filter6d, 4.0/filter6d,
        6.0/filter6d, 24.0/filter6d, 36.0/filter6d, 24.0/filter6d, 6.0/filter6d,
        4.0/filter6d, 16.0/filter6d, 24.0/filter6d, 16.0/filter6d, 4.0/filter6d,
        1.0/filter6d, 4.0/filter6d, 6.0/filter6d, 4.0/filter6d, 1.0/filter6d
    };
    
    double filter7[25];
    
    int o;
    for(o=0;o<25;o++){
        filter7[o] = 1.0/25.0;
    }
    
    double filter8[49];
    for(o=0;o<49;o++){
        filter8[o] = 1.0/25.0;
    }
    
       
    CvMat filterMat = cvMat(3, 3, CV_64FC1, filter0);
    
    // as long as there are images ...
    while (src_image = cvQueryFrame (camera))
    {
        
        
        int j,k,l,m;
        
        //Quad View
        if(quadImages){
            cvFlip (src_image, current_frame, 1);
            int nrow = 0;
            for(l = 0; l<height; l+=2){
                int ncol = 0;
                for(m = 0; m<arrayWidth; m+=6){
                    uchar p11b = CV_IMAGE_ELEM(current_frame, uchar, l, m);
                    uchar p11g = CV_IMAGE_ELEM(current_frame, uchar, l, m+1);
                    uchar p11r = CV_IMAGE_ELEM(current_frame, uchar, l, m+2);
                    
                    uchar p12b = CV_IMAGE_ELEM(current_frame, uchar, l, m+3);
                    uchar p12g = CV_IMAGE_ELEM(current_frame, uchar, l, m+4);
                    uchar p12r = CV_IMAGE_ELEM(current_frame, uchar, l, m+5);
                    
                    uchar p21b = CV_IMAGE_ELEM(current_frame, uchar, l+1, m);
                    uchar p21g = CV_IMAGE_ELEM(current_frame, uchar, l+1, m+1);
                    uchar p21r = CV_IMAGE_ELEM(current_frame, uchar, l+1, m+2);
                    
                    uchar p22b = CV_IMAGE_ELEM(current_frame, uchar, l+1, m+3);
                    uchar p22g = CV_IMAGE_ELEM(current_frame, uchar, l+1, m+4);
                    uchar p22r = CV_IMAGE_ELEM(current_frame, uchar, l+1, m+5);
                    
                    uchar pb = (p11b+p12b+p21b+p22b) >> 2;
                    uchar pg = (p11g+p12g+p21g+p22g) >> 2;
                    uchar pr = (p11r+p12r+p21r+p22r) >> 2;
                    int y = 0.257*pr + 0.504*pg + 0.098*pb + 16; 
                    
                    if(y < 100){
                        CV_IMAGE_ELEM(quad_image, uchar, nrow, ncol) = 0;
                        CV_IMAGE_ELEM(quad_image, uchar, nrow, ncol+1) = 0;
                        CV_IMAGE_ELEM(quad_image, uchar, nrow, ncol+2) = 0xFF;
                        
                        CV_IMAGE_ELEM(quad_image, uchar, nrow, ncol+arrayWidth2) = 0xFF;
                        CV_IMAGE_ELEM(quad_image, uchar, nrow, ncol+1+arrayWidth2) = 0;
                        CV_IMAGE_ELEM(quad_image, uchar, nrow, ncol+2+arrayWidth2) = 0;
                        
                        CV_IMAGE_ELEM(quad_image, uchar, nrow+height2, ncol) = 0;
                        CV_IMAGE_ELEM(quad_image, uchar, nrow+height2, ncol+1) = 0;
                        CV_IMAGE_ELEM(quad_image, uchar, nrow+height2, ncol+2) = 0;
                                                
                        CV_IMAGE_ELEM(quad_image, uchar, nrow+height2, ncol+arrayWidth2)= 0;
                        CV_IMAGE_ELEM(quad_image, uchar, nrow+height2, ncol+1+arrayWidth2) = 0xFF;
                        CV_IMAGE_ELEM(quad_image, uchar, nrow+height2, ncol+2+arrayWidth2) = 0;
                        
                    } else {
                        CV_IMAGE_ELEM(quad_image, uchar, nrow, ncol) = 0;
                        CV_IMAGE_ELEM(quad_image, uchar, nrow, ncol+1) = 0xFF;
                        CV_IMAGE_ELEM(quad_image, uchar, nrow, ncol+2) = 0;
                        
                        CV_IMAGE_ELEM(quad_image, uchar, nrow, ncol+arrayWidth2) = 0;
                        CV_IMAGE_ELEM(quad_image, uchar, nrow, ncol+1+arrayWidth2) = 0;
                        CV_IMAGE_ELEM(quad_image, uchar, nrow, ncol+2+arrayWidth2) = 0xFF;
                        
                        CV_IMAGE_ELEM(quad_image, uchar, nrow+height2, ncol) = 0xFF;
                        CV_IMAGE_ELEM(quad_image, uchar, nrow+height2, ncol+1) = 0;
                        CV_IMAGE_ELEM(quad_image, uchar, nrow+height2, ncol+2) = 0;
                        
                        CV_IMAGE_ELEM(quad_image, uchar, nrow+height2, ncol+arrayWidth2)= 0;
                        CV_IMAGE_ELEM(quad_image, uchar, nrow+height2, ncol+1+arrayWidth2) = 0;
                        CV_IMAGE_ELEM(quad_image, uchar, nrow+height2, ncol+2+arrayWidth2) = 0;
                    }
                    ncol += 3;
                }
                nrow++;
            }
        } else {
            cvFlip (src_image, quad_image, 1);
        }
        
        //Filter Matrix
        int rangeMax = (&filterMat)->rows/2;
        int rangeMin = rangeMax*-1;
        for(l = 0; l<height; l++){
            for(m = 0; m<arrayWidth; m+=3){
                int bNew = 0;
                int gNew = 0;
                int rNew = 0;
                
                for(j = rangeMin; j<= rangeMax; j++){
                    for(k = rangeMin; k<=rangeMax; k++){
                        int k3 = 3*k;
                        int r = l+j;
                        int c = m+k3;
                        if(r >= 0 && c >= 0){
                            uchar b = CV_IMAGE_ELEM(quad_image, uchar, l+j, m+k3);
                            uchar g = CV_IMAGE_ELEM(quad_image, uchar, l+j, m+k3+1);
                            uchar r = CV_IMAGE_ELEM(quad_image, uchar, l+j, m+k3+2);
                            
                            float factor = cvmGet(&filterMat, j+rangeMax, k+rangeMax);
                            if(factor != 0){
                                bNew += b * factor;
                                gNew += g * factor;
                                rNew += r * factor;
                            }
                        }
                    }
                }
                if(bNew < 0) bNew = 0;
                if(gNew < 0) gNew = 0;
                if(rNew < 0) rNew = 0;
                
                CV_IMAGE_ELEM(draw_image, uchar, l, m) = bNew;
                CV_IMAGE_ELEM(draw_image, uchar, l, m+1) = gNew;
                CV_IMAGE_ELEM(draw_image, uchar, l, m+2) = rNew;
            }
        }
        
        // display the image
        cvShowImage (WINDOW_NAME, draw_image);
        
        
        
        // keyboard menu
        int key = cvWaitKey (10);
        if (key == '0'){
            filterMat = cvMat(3, 3, CV_64FC1, filter0);
        }
        if (key == '1'){
            filterMat = cvMat(3, 3, CV_64FC1, filter1);
        }
        if (key == '2'){
            filterMat = cvMat(3, 3, CV_64FC1, filter2);
        }
        if (key == '3'){
            filterMat = cvMat(3, 3, CV_64FC1, filter3);
        }
        if (key == '4'){
            filterMat = cvMat(3, 3, CV_64FC1, filter4);
        }
        if (key == '5'){
            filterMat = cvMat(3, 3, CV_64FC1, filter5);
        }
        if (key == '6'){
            filterMat = cvMat(5, 5, CV_64FC1, filter6);
        }
        if (key == '7'){
            filterMat = cvMat(5, 5, CV_64FC1, filter7);
        }
        if (key == '8'){
            filterMat = cvMat(7, 7, CV_64FC1, filter8);
        }
        if (key == 't'){
            quadImages = !quadImages;
        }
        
        if (key == 'q' || key == 'Q')
            break;
    }
    
    return 0;
}