#include "cv.h"
#include "highgui.h"
#include <stdio.h>

IplImage *image = 0, *timg = 0, *gray = 0, *pyr = 0, *tgray = 0;
CvMemStorage *storage;

int threshold = 10;

struct point {
    int x, y;
    int vx, vy;
};

struct line {
    int x1, y1, x2, y2;
};

CvSeq* findContours(IplImage *image, IplImage **draw) {

        CvSize sz = cvSize(image->width & -2, image->height & -2);
        timg = cvCloneImage(image);
        gray = cvCreateImage(sz, 8, 1);
        pyr = cvCreateImage(cvSize(sz.width/2, sz.height/2), 8, 3);
        tgray = cvCreateImage(sz, 8, 1);

        cvSetImageROI(timg, cvRect(0, 0, sz.width, sz.height));

        //cvPyrDown(timg, pyr, 7);
        //cvPyrUp(pyr, timg, 7);

        cvSetImageCOI(timg, 1);
        cvCopy(timg, tgray, 0);

        //cvCanny(tgray, gray, 0, 10, 5);
        cvThreshold(tgray, gray, 255/(threshold+1), 255, CV_THRESH_BINARY);
        //cvAdaptiveThreshold(tgray, gray, threshold);

        CvSeq *contours;

        cvFindContours(gray, storage, &contours, sizeof(CvContour));

        *draw = image;

        return contours;
}

int collision(struct point *ball, struct line *line) {

    //printf("ball %d, %d against line %d, %d -> %d, %d\n",
    //        ball->x, ball->y, line->x1, line->y1, line->x2, line->y2);

    float ball_radius = 10;

    float R = ball_radius + sqrt(ball->vx * ball->vx + ball->vy * ball->vy);
    
    //float R = ball_radius;

    float lx2 = line->x2;
    float lx1 = line->x1;
    float ly2 = line->y2;
    float ly1 = line->y1;

    float px = ball->x;
    float py = ball->y;

    float dx, dy, ld, lux, luy, lnx, lny, dx1, dy1, d, dx2, dy2, dot1, dot2, distsq;

    dx = lx2 - lx1;
    dy = ly2 - ly1;
    ld = sqrt(dx*dx + dy*dy);
    lux = dx / ld;
    luy = dy / ld;
    lnx = luy;
    lny = -lux;
    dx1 = px - (lx1 - lux * R);
    dy1 = py - (ly1 - luy * R);
    d = sqrt(dx1*dx1 + dy1*dy1);
    dx1 = dx1 / d;
    dy1 = dy1 / d;
    dx2 = px - (lx2 + lux * R);
    dy2 = py - (ly2 + luy * R);
    d = sqrt(dx2*dx2 + dy2*dy2);
    dx2 = dx2 / d;
    dy2 = dy2 / d;
    dot1 = dx1 * lux + dy1 * luy;
    dot2 = dx2 * lux + dy2 * luy;
    px = lx1 - px;
    py = ly1 - py;
    distsq = fabs((dx * py - px * dy) / ld);
    
    return ((dot1>=0 && dot2<=0) || (dot1<=0 && dot2>=0)) && (distsq <= R);

}

int sign(float x) {
    if(x > 0) return 1;
    else if(x == 0) return 0;
    else if (x < 0) return -1;
}

int do_physics(struct point *ball, struct line *environment) {

    int collidesegment = -1;

    ball->vy += 1;

    ball->x += ball->vx;
    ball->y += ball->vy;

    //printf("%d, %d\n", ball->x, ball->y);
    
    int i;

    //double bvx, bvy, bv, dx, dy, lineangle, ballangle, incidence, deflection;
    
    float bvx = ball->vx;
    float bvy = ball->vy;

    for(i = 0; i < 100; i++) {
        if(environment[i].x1 == 0) break;
        if(collision(ball, environment + i)) {

            float ldy = environment[i].y2 - environment[i].y1;
            float ldx = environment[i].x2 - environment[i].x1;

            float ll = sqrt(ldy*ldy+ldx*ldx);

            /*printf("ldx: %f, ldy: %f, ll: %f\n", ldx, ldy, ll);

            float lnx = ldy;
            float lny = -ldx;

            float dp1 = bvx * ldx + bvy * ldy;

            printf("bvx: %f, bvy: %f\n", bvx, bvy);
            printf("lnx: %f, lny: %f, dp1: %f\n", lnx, lny, dp1);
            
            float proj1x = dp1 * ldx / ll;
            float proj1y = dp1 * ldy / ll;

            printf("proj1x: %f, proj1y: %f\n", proj1x, proj1y);

            float dp2 = bvx * lnx + bvy * lny;

            float proj2x = dp2 * lnx / ll;
            float proj2y = dp2 * lny / ll;

            printf("proj2x: %f, proj2y: %f\n", proj2x, proj2y);

            proj2x *= -1;
            proj2y *= -1;

            ball->vx = proj1x + proj2x;
            ball->vy = proj1y + proj2y;*/

            float dp = bvx * ldx + bvy * ldy;
            float mag = sqrt(bvx*bvx+bvy*bvy) * sqrt(ldx*ldx+ldy*ldy);
            if(mag < 1) mag = 3;
            float ang = acos(dp/mag);
            float cosine = dp/mag;
            float sine = sin(ang);
            ball->vx = (bvy * cosine + bvx * sine);
            ball->vy = (bvx * cosine - bvy * sine);

            collidesegment = i;

            break;

        }
    }

    if(ball->y > 400) {
        ball->y = 0;
        ball->vy = 0;
    }

    if(ball->y < 0) {
        //ball->y = ball->y;
        //ball->vy = -ball->vy;
        ball->y = 400;
    }

    if(ball->x > 600) {
        ball->x = 1;
    }


    if(ball->x <= 0) {

        //ball->x = -ball->x;
        //ball->vx = -ball->vx;
        ball->x = 600;
        //printf("hello!\n");
    }

    //printf("%d\n\n", ball->x);

    
    return collidesegment;

    //printf("returning\n");

}

int main(int argc, char** argv) {

    CvCapture* cap = 0;
    cap = cvCaptureFromCAM(0);

    storage = cvCreateMemStorage(0);

    cvNamedWindow("ImageWindow", 1);
    cvCreateTrackbar("Threshold", "ImageWindow", &threshold, 40, 0);

    struct line *environment = new line[1000];

    struct point *ball = new point;

    ball->x = 300;
    ball->y = 10;
    ball->vx = 0;
    ball->vy = 0;

    while(1) {

        IplImage *frame = 0;
        frame = cvQueryFrame(cap);

        if(!frame) {
            continue;
        }

        if(!image) {
            image = cvCreateImage (cvGetSize(frame), 8, 3);
            image->origin = frame->origin;
        }

        cvCopy(frame, image, 0);

        IplImage *draw;

        CvSeq *result;
        CvSeq *contours = findContours(image, &draw);

        int i, n;
        
        n = 0;

        while(contours) {

           result = cvApproxPoly(contours, sizeof(CvContour), storage,
                  CV_POLY_APPROX_DP, cvContourPerimeter(contours)*0.01, 0);

           for(i = 0; i < result->total-1; i++) {

               CvPoint p1 = *((CvPoint*)cvGetSeqElem(result, i));
               CvPoint p2 = *((CvPoint*)cvGetSeqElem(result, i+1));

               if (p1.x < 10 || p1.x > draw->width - 10
                       || p2.x < 10 || p2.x > draw->width - 10
                       || p1.y < 10 || p1.y > draw->height - 10
                       || p2.y < 10 || p2.y > draw->height - 10) {
                   continue;
               }

               if (((p2.x - p1.x)*(p2.x - p1.x) + (p2.y - p1.y)*(p2.y - p1.y)) < 100) {
                   continue;
               }

               cvLine(draw, p1, p2, CV_RGB(255,0,0), 3, 8);

               environment[n].x1 = p1.x;
               environment[n].y1 = p1.y;
               environment[n].x2 = p2.x;
               environment[n].y2 = p2.y;
               //printf("%d, %d, %d, %d\n", environment[n].x1, environment[n].y1, environment[n].x2, environment[n].y2);

               n++;

           }

           contours->h_next;
           contours = contours->h_next;

        }

        for(i = n; i < 100; i++) {
            environment[i].x1 = 0;
            //environment[i].y1 = 0;
            //environment[i].x2 = 0;
            //environment[i].y2 = 0;
        }

        int collide = do_physics(ball, environment);

        cvCircle(draw, cvPoint(ball->x, ball->y), 5, CV_RGB(0,0,255), 3, 8);

        if(collide != -1) {
            //printf("%d\n", collide);
            cvLine(draw, cvPoint(environment[collide].x1,
                                 environment[collide].y1),
                         cvPoint(environment[collide].x2,
                                 environment[collide].y2),
                         CV_RGB(0,255,0), 3, 8);
        }

        cvShowImage("ImageWindow", image);

        int c = cvWaitKey(10);

        if ((char)c == 27)
            break;

        //if(bally > image->height) bally = 0;

        cvReleaseImage(&image);
        cvReleaseImage(&timg);
        cvReleaseImage(&gray);
        cvReleaseImage(&pyr);
        cvReleaseImage(&tgray);

    }

    printf("Release\n");

    cvReleaseCapture(&cap);
    cvDestroyWindow("ImageWindow");

}