/*
 廊下の検出
 */
#include <cv.h>
#include <highgui.h>
#include <math.h>

CvPoint  mid_point(CvPoint p1,CvPoint p2){
	return cvPoint((p1.x+p2.x)/2, (p1.y+p2.y)/2);
}
CvPoint offset_point(CvPoint p,int x,int y){
	return cvPoint(p.x + x , p.y + y);
}
void edge_points(float rho,float theta,CvSize size,CvPoint *pt1,CvPoint *pt2){
	double a = cos(theta), b = sin(theta);
	double x0 = a*rho, y0 = b*rho;
	double grad = tan(3.1415/2 - theta);
	double intercept = y0+grad*x0;

	pt1->x = intercept/grad;
	pt1->y = 0;

	if(grad>0){
		pt2->x = 0;
		pt2->y = intercept;
	}else{
		pt2->x = size.width;
 		pt2->y = -size.width*grad + intercept;
	}
}
int main(int argc, char** argv)
{
	IplImage *src,*image, *sobel , *dst ,*color_dst;
	CvRect roi ;
	CvSize size;
	int i;

	image  = cvLoadImage("passage.ppm",1);
	
	cvNamedWindow( "Sobel", 1 );
	cvNamedWindow( "Hough", 1 );

	//下半分に廊下があると考えて、画面の下半分をROIに指定
	roi = cvRect(0,image->height/2,image->width,image->height/2);
	size = cvSize(roi.width,roi.height);

	src = cvCreateImage(size,8,1);
	dst = cvCreateImage( size , 8, 1);
	color_dst  = cvCreateImage( cvGetSize(image) , 8, 3);
	sobel = cvCreateImage(size,IPL_DEPTH_16S,1);
	
	//出力画像
	cvCopy(image,color_dst,NULL);

	cvSetImageROI(image,roi);

	//グレー画像にする
	cvCvtColor(image,src,CV_BGR2GRAY);

	//x-方向に微分して、エッジを出す
	cvSobel(src,sobel,0,1,3);
	cvConvertScaleAbs(sobel,dst,1,0);
	cvThreshold(dst,dst,40,255,CV_THRESH_BINARY);

	cvShowImage("Sobel",dst);
	
	// エッジ画像に対してハフ変換を行う
	CvMemStorage* storage = cvCreateMemStorage(0);
	CvSeq* lines = 0;

	lines = cvHoughLines2( dst, storage, CV_HOUGH_STANDARD, 
			       1, CV_PI/180, 100, 0, 0 );
	CvPoint pt1[2],pt2[2];

	// もっとも投票数の多かった2つの直線を"道沿いの線"と仮定して
	// (linesには投票結果が入っている)
	for( i = 0; i < 2; i++ )
	{
		float* param = (float*)cvGetSeqElem(lines,i);
		// paramはρ-θ空間の値
		// edge_pointsは画面中の直線の端点を計算する
		edge_points(param[0],param[1],size,&pt1[i],&pt2[i]);
		//ラインを描く
		cvLine( color_dst, 
			cvPoint(pt1[i].x+roi.x, pt1[i].y+roi.y),
			cvPoint(pt2[i].x + roi.x , pt2[i].y + roi.y),
			CV_RGB(255,0,0), 2, 8,0 );
	}
	// センターラインを描く
	cvLine( color_dst, 
		offset_point(mid_point(pt1[0],pt1[1]), roi.x ,roi.y),
		offset_point(mid_point(pt2[0],pt2[1]), roi.x ,roi.y),
		CV_RGB(0,255,0), 3, 8,0);
	
	cvShowImage( "Hough", color_dst );
	cvWaitKey(0);
		
}