#include<highgui.h>
#include<cv.h>
#include<stdio.h>
#include<float.h>
#include<conio.h>

char *origImage="Original Image";
char *cannyWindow="Canny Filter's Output";
char *houghOutput="Lines detected by the Hough Transform";

char *cannyThreshhold="Canny Threshhold";

char *rhoBar="Rho";
char *thetaBar="Theta";
char *houghThreshholdBar="Hough Threshhold";
char *minLineLengthBar="Minimum Line Length";
char *maxDistBar="Max distance apart";
char *vanPointsThreshBar="Accumulator Threshhold";

IplImage *im, *grayIm, *edgeIm, *houghImage;
CvMemStorage *storage;
CvSeq *lines;
int threshhold, rho, theta, houghThreshhold, minLineLength, maxDist, vanPointsThresh;

CvSeq* vanishingPoints(CvSeq *lines, CvSize im, int xlimit, int ylimit, int res, int accumThresh)
{        
         /*AUTHOR: RAVI PRAKASH
           UNIVERSITY OF SOUTHERN CALIFORNIA
           lines: Sequence of lines obtained from houghLines2()
           im: CvSize of the original image
           xlimit: \ The dimensions of the area that is to be mapped linearly and not logarithmically. 
           ylimit: /
           res: The factor of reduction in the linearly mapped region. i.e. if res=5, a 5x5 area of the image will correspond to a cell in the linear region
           accumThresh: Threshhold above which value of the bin in the accumulator is considered a peak            
         
           Returns: CvSeq of int arr[3]
           arr[0] contains x and arr[1] contains y of the peak
           arr[2] contains the number of votes in the bin
         */
         int dimx=(int) ((1.0*xlimit)/res+log10(DBL_MAX)), dimy=(int) ((1.0*ylimit)/res+log10(DBL_MAX));
         printf("\nNo. of Lines : %d\ndimx=%d and dimy=%d\n", lines->total, dimx, dimy);
//         getch();
         int accumulator[dimx][dimy];
         for(int i=0; i<dimx; i++)
                 for(int j=0; j<dimy; j++)
                         accumulator[i][j]=0;
         //All pairs lines
         int m1, m2;
         double xconst, yconst;
         xconst=xlimit/(2.0*res)-log10(xlimit/2.0)+1;
         yconst=ylimit/(2.0*res)-log10(ylimit/2.0)+1;
         
         for(int i=0; i<lines->total-1; i++)
         {
                for(int j=i+1; j<lines->total; j++)
                {
                        double x, y;
                        //Finding Intersection of the two lines
                        CvPoint* pts1 = (CvPoint*) cvGetSeqElem(lines,i);
                        CvPoint* pts2 = (CvPoint*) cvGetSeqElem(lines,j);
                        if(pts1[0].x!=pts1[1].x)                        
                        {                                
                                m1=(pts1[0].y-pts1[1].y)/(pts1[0].x-pts1[1].x);
                                if(pts2[0].x!=pts2[1].x)
                                {
                                        m2=(pts2[0].y-pts2[1].y)/(pts2[0].x-pts2[1].x);                                        
                                        //           c2           -           c1               / m1-m2 = x of intersection of the lines
                                        if(m1!=m2)
                                            x=(1.0*pts2[0].y-m2*pts2[0].x)-(pts1[0].y-m1*pts1[0].x)/(m1-m2);
                                        else continue;
                                }
                                else 
                                        x=pts2[0].x;                                                                      
                                y=m1*x+(pts1[0].y-m1*pts1[0].x); 
                        }
                        else 
                        {
                                x=pts1[0].x;
                                if(pts2[0].x!=pts2[1].x)
                                {
                                        m2=(pts2[0].y-pts2[1].y)/(pts2[0].x-pts2[1].x);
                                }
                                else continue;
                                                                     
                                y=m1*x+(pts1[0].y-m1*pts1[0].x);
                        }
                        // x and y are the intersection points of the pair of lines in current iteration. Now mapping to accumulator space
                        x=x-(im.width)/2;
                        y=y-(im.height)/2;
                        
                        int xindex, yindex;
                        
                        //NOTE: Here we can impose the restriction that xlimit and ylimit HAVE to be even; otherwise linear mapping will extend slightly less (0.5) than intended
                        if((int)fabs(x)<=xlimit/2)                        
                             xindex=(int) x/res;
                        else
                        {
                                    if(x>0)
                                         xindex= (int) ( log10(x)+xconst );
                                    else
                                         xindex= (int) ( log10(-1.0*x)-xconst );
                        }
                        xindex+=dimx/2;
                                                                 
                        if((int)fabs(y)<=ylimit/2)                        
                             yindex=(int) y/res;
                        else
                        {
                                    if(y>0)
                                         yindex= (int) ( log10(y)+yconst );
                                    else
                                         yindex= (int) ( log10(-1.0*y)-yconst );
                        }
                        yindex+=dimy/2;


                        if(xindex<0 || yindex<0 || xindex>dimx || yindex>dimy) 
                                    printf("(%d, %d)\t", xindex, yindex);             
                        
                        //incrementing bin in accumulator to vote for intersection in that area
                        accumulator[xindex][yindex]++;
                }
        }

        
        CvMemStorage *vPointsMem=cvCreateMemStorage(64); //Initial block size = 64 bytes
        CvSeq *vPoints=cvCreateSeq(0, sizeof(CvSeq), 3*sizeof(int), vPointsMem);
        int temp[3];
// CODE TO SORT THROUGH PEAKS        
        int i, j;
        for(int cnti=0; cnti<dimx; cnti++)
             for(int cntj=0; cntj<dimy; cntj++)
             {
                  if(accumulator[cnti][cntj]>accumThresh)
                  {
                        //Translate back from accumulator space to image space. Phew :(                            
                       i=cnti-dimx/2;
                       j=cntj-dimy/2;              
                       
                       if ( abs(i) < xlimit/2 )
                            temp[0]=i*res;
                       else
                       {
                           if(i>0)
                                temp[0]=(int) pow(10, i-xconst);
                           else 
                                temp[0]=(int) ( -1.0*pow(10, -1.0*i+xconst) );
                       }    
                       
                       if ( abs(j) < ylimit/2 )
                            temp[1]=j*res;
                       else
                       {
                           if(j>0)
                                temp[1]=(int) pow(10, j-yconst);
                           else 
                                temp[1]=(int) ( -1.0*pow(10, -1.0*j+yconst) );
                       }
                       
                       temp[0]=temp[0]+(im.width)/2;
                       temp[1]=temp[1]+(im.height)/2;

                       temp[2]=accumulator[cnti][cntj];
                       printf("(%d, %d) = %d\n", temp[0], temp[1], temp[2]);
                       cvSeqPush(vPoints, temp);
                  }
             }
         return vPoints;

}





void findHoughLines(int arbit)
{
    lines = cvHoughLines2(edgeIm, storage, CV_HOUGH_PROBABILISTIC, 1+rho, 1+theta, 1+houghThreshhold, 1+minLineLength, 1+maxDist);
    houghImage=cvCreateImage( cvSize(im->width, im->height), im->depth, 3);    
     for(int i=0; i<lines->total; i++)
     {
            CvPoint* pts = (CvPoint*) cvGetSeqElem(lines,i);
            cvLine(houghImage , pts[0], pts[1], CV_RGB(255,0,0), 1, 8 );
     }
     cvShowImage(houghOutput, houghImage);
     CvSeq *vPoints=vanishingPoints(lines, cvSize(im->width, im->height),2*(im->width), 2*(im->height), 50, vanPointsThresh);
}
 
void findEdge(int threshhold)
{    
    cvCanny(grayIm, edgeIm, 1.0*threshhold, 3.0*threshhold, 3);
    cvShowImage(cannyWindow, edgeIm);
    findHoughLines(2);
}

int main()
{    
    im = cvLoadImage("3.jpg", CV_LOAD_IMAGE_COLOR);
    cvNamedWindow(origImage, CV_WINDOW_AUTOSIZE);
    cvShowImage(origImage, im);
    
    threshhold=350;
    vanPointsThresh=1000;

    cvNamedWindow(cannyWindow);
    cvCreateTrackbar(cannyThreshhold, cannyWindow,  &threshhold, 350, findEdge);

    cvNamedWindow(houghOutput);
    storage = cvCreateMemStorage(0);
    cvCreateTrackbar(rhoBar, houghOutput,  &rho, 100, findHoughLines);
    cvCreateTrackbar(thetaBar, houghOutput,  &theta, 100, findHoughLines);
    cvCreateTrackbar(houghThreshholdBar, houghOutput,  &houghThreshhold, 100, findHoughLines);
    cvCreateTrackbar(minLineLengthBar, houghOutput,  &minLineLength, 100, findHoughLines);
    cvCreateTrackbar(maxDistBar, houghOutput,  &maxDist, 100, findHoughLines);
    cvCreateTrackbar(vanPointsThreshBar, houghOutput,  &vanPointsThresh, 20000, findHoughLines);
    
    grayIm=cvCreateImage( cvSize(im->width, im->height), im->depth, 1);
    cvCvtColor(im, grayIm, CV_RGB2GRAY);
    edgeIm=cvCreateImage( cvSize(im->width, im->height), im->depth, 1);
   
    cvWaitKey(0);
    cvReleaseImage(&im);
    cvDestroyWindow(origImage);
    cvReleaseImage(&grayIm);
    cvReleaseImage(&edgeIm);    
    cvDestroyWindow(cannyWindow);
    
}