#ifndef __MYCVGL__
#define __MYCVGL__

/*=============================================================
	myCVGL.h
	OpenCVとOpenGLの基本的な機能をまとめときます
=============================================================*/

////////////////////////////////////////////////////////////////
//ヘッダ

#include <glut.h>
#include "cxcore.h"
#include "cv.h"
#include "highgui.h"
#pragma comment(lib,"cv.lib")
#pragma comment(lib,"cxcore.lib")
#pragma comment(lib,"highgui.lib")

////////////////////////////////////////////////////////////////
//マクロ定義

#define W_MAX 320           //画像サイズ
#define H_MAX 240
#define THRESHOLD 50        //重心更新の閾値
#define IGNORE_DOT 10       //この値以上連続しない値はノイズ扱い

#ifndef PI
#define PI (4*atan(1.0))
#endif

////////////////////////////////////////////////////////////////
//プロトタイプ宣言

void initTexture(IplImage *image);
void getpixel(IplImage *image, int x, int y, int *r, int *g, int *b);
int getpixel1(IplImage *image, int x, int y);
void putpixel(IplImage *image, int x, int y, int *r, int *g, int *b);
int getgrav(IplImage *image,int *gx,int *gy);
void setgrav(IplImage *image,int gx,int gy);
void cam_open(void);
void cam_read(void);
void cam_close(void);
int diff_OneShot(void);
void myDiffMainLoop(int ResetLevel);
void axis(void);
void initTexture(IplImage *image);
void getTexture(void);
void myBox(double x,double y,double z);
void myCylinder(double radius,double height,double sides);
void TimeMesureStart(int PN);
double TimeMesureEnd(int PN);

////////////////////////////////////////////////////////////////
//OpenCV関連
CvCapture* capture;
IplImage *now,*bg,*diff,*object,*gray;
int gx,gy;
int rst=10;

/*=========================================================================
【関数】getpixel
【用途】指定画素のRGB値を取得する
【引数】
		*image		対象のiplImageへのポインタ
		x,y			xy座標の指定
		*r,*g,*b	取得したRGB画素が返されるポインタ

【戻値】なし
【仕様】RGB各8bit，24bit画像が対象
=========================================================================*/

void getpixel(IplImage *image, int x, int y, int *r, int *g, int *b)
{
    *r =(uchar) image->imageData[y *image->widthStep+ x * image->nChannels + 2];
    *g =(uchar) image->imageData[y *image->widthStep+ x * image->nChannels + 1];
    *b =(uchar) image->imageData[y *image->widthStep+ x * image->nChannels    ];
}



/*=========================================================================
【関数】getpixel1
【用途】指定画素のグレイスケール値を取得する
【引数】
		*image		対象のiplImageへのポインタ
		x,y			xy座標の指定

【戻値】指定画素のグレイスケール値
【仕様】グレイスケール画像が対象
=========================================================================*/

int getpixel1(IplImage *image, int x, int y)
{
    return image->imageData[y *image->widthStep+x * image->nChannels];
}



/*=========================================================================
【関数】putpixel
【用途】指定画素にRGB値を書き込む
【引数】
		*image		対象のiplImageへのポインタ
		x,y			xy座標の指定
		*r,*g,*b	書き込むRGB画素が格納されるポインタ

【戻値】なし
【仕様】RGB各8bit，24bit画像が対象
=========================================================================*/

void putpixel(IplImage *image, int x, int y, int *r, int *g, int *b)
{
    image->imageData[y *image->widthStep+ x * image->nChannels + 2] = *r;
    image->imageData[y *image->widthStep+ x * image->nChannels + 1] = *g;
    image->imageData[y *image->widthStep+ x * image->nChannels    ] = *b;
}



/*=========================================================================
【関数】getgrav
【用途】差分画像の重心取得
【引数】
		*image		対象のiplImageへのポインタ
		*x,*y		xy座標の指定のポインタ

【戻値】画像のrst(%)が更新されていれば１，いなければ0
【仕様】グレイスケール画像が対象，画像のrst(%)が更新されていれば１
=========================================================================*/

int getgrav(IplImage *image,int *gx,int *gy){
    int i,j,temp=0;
    double x_temp=0,y_temp=0,m=0;
    for(i=0;i<W_MAX;i++){
        for(j=0;j<H_MAX;j++){
            if(getpixel1(image,i,j)){
                temp++;
                if(temp>IGNORE_DOT){
                    x_temp+=i*IGNORE_DOT-IGNORE_DOT*0.5;
                    y_temp+=j*IGNORE_DOT-IGNORE_DOT*0.5;
                    m+=IGNORE_DOT;
                    temp=0;
                }
            }
            else temp=0;
        }
    }

    if(m>THRESHOLD){
        *gx=x_temp/m;
        *gy=y_temp/m;
    }else{
            //*gx=W_MAX/2;      //中心に戻る?
            //*gy=H_MAX/2;
    }
    return m>(W_MAX*H_MAX*rst)/100?1:0;
}



/*=========================================================================
【関数】setgrav
【用途】画像に重心の書き込み
【引数】
		*image		対象のiplImageへのポインタ
		gx,gy		xy座標の指定

【戻値】なし
【仕様】重心に20×20画素を書き込む
=========================================================================*/

void setgrav(IplImage *image,int gx,int gy){
    int i,j;
    int R,G,B;
    R=G=B=255;
    for(j=-10;j<10;j++){
        for(i=-10;i<10;i++){
            if(gx+i<W_MAX&&gy+j<H_MAX&&gx+i>=0&&gy+j>=0){
                putpixel(image,gx+i,gy+j,&R,&G,&B);
            }
        }
    }
}

/*=========================================================================
【関数】cam_open();
【用途】カメラを開く
【引数】なし
【戻値】なし
【仕様】ストリームはcapture
=========================================================================*/
void cam_open(){
    capture = cvCaptureFromCAM(0);
    cvNamedWindow("CAMERA",1);
}



/*=========================================================================
【関数】cam_open();
【用途】カメラから画像を読み込み
【引数】なし
【戻値】なし
【仕様】nowに格納
=========================================================================*/
void cam_read(){
    now    = cvQueryFrame(capture);
    cvShowImage("CAMERA",now);
}



/*=========================================================================
【関数】cam_close();
【用途】カメラを閉じる
【引数】なし
【戻値】なし
【仕様】
=========================================================================*/
void cam_close(){
    cvReleaseImage(&now);
    cvReleaseCapture(&capture);
    cvDestroyWindow("CAMERA");
}



/*=========================================================================
【関数】diff_OneShot
【用途】差分処理本体
【引数】なし
【戻値】キーボード入力
【仕様】グローバル変数gx,gyに重心座標を返す
=========================================================================*/
int diff_OneShot(){
    int cnt,key;
    static int cvflg=1;


    if(cvflg){
        capture = cvCaptureFromCAM(0);
        now    = cvQueryFrame(capture);
        bg     = cvCreateImage(cvSize(now->width,now->height),IPL_DEPTH_8U,3);
        diff   = cvCreateImage(cvSize(now->width,now->height),IPL_DEPTH_8U,3);
        object = cvCreateImage(cvSize(now->width,now->height),IPL_DEPTH_8U,3);
        gray   = cvCreateImage(cvSize(now->width,now->height),IPL_DEPTH_8U,1);
        cvCopy(now,bg,NULL);
        cvNamedWindow("diff",1);
        cvflg=0;
    }

    cvSetZero(object);
    now    = cvQueryFrame(capture);
    cvAbsDiff(now,bg,diff);

    cvCvtColor(diff,gray,CV_BGR2GRAY);
    cvThreshold(gray,gray,30,1,CV_THRESH_BINARY);
    cvCopy(now,object,gray);

    if(getgrav(gray,&gx,&gy))cvCopy(now,bg,NULL);
	setgrav(object,gx,gy);

    cvConvertImage(object,object,1);

    cvShowImage("diff",object);

    switch(key=cvWaitKey(10)){
       case 'r':   cvCopy(now,bg,NULL);
                   break;
    }

    return key;

}



/*=========================================================================
【関数】myDiffMainLoop
【用途】簡易差分画像取得
【引数】
        ResetLevel  画像の何％が更新されたら背景画像を入れ替えるか
                    0で逐次更新

【戻値】なし
【仕様】グローバル変数gx,gyに重心座標を返す
=========================================================================*/

void myDiffMainLoop(int ResetLevel){
        int cnt,key;
        rst=ResetLevel;


        while(diff_OneShot()!='q');

        cvReleaseCapture(&capture);
        cvReleaseImage(&diff);
        cvReleaseImage(&object);
        cvReleaseImage(&gray);
        cvReleaseImage(&bg);
        cvReleaseImage(&now);
        cvDestroyWindow("diff");
        exit(0);
}




////////////////////////////////////////////////////////////////
//OpenGL関連

#define TEX_HEIGHT 240
#define TEX_WIDTH  320

GLfloat glwhite[] = {1.0,1.0,1.0,1.0};
GLfloat glred[]   = {0.8,0.0,0.0,1.0};
GLfloat glgreen[] = {0.0,0.8,0.0,1.0};
GLfloat glblue[]  = {0.0,0.0,0.8,1.0};
GLfloat glblack[] = {0.2,0.2,0.2,1.0};
GLfloat glgray[]  = {0.7,0.7,0.7,1.0};

/*=========================================================================
【関数】axis
【用途】x,y,z軸の描画
【引数】
		length 軸の長さ

【戻値】なし
【仕様】なし
=========================================================================*/

void axis(){
	glPushMatrix();
	glMaterialfv(GL_FRONT,GL_DIFFUSE,glred);
	glColor3d( 1.0, 0.0, 0.0);		//X軸は赤
	glBegin(GL_LINES);
		glVertex3f(-10,  0,  0);
		glVertex3f( 10,  0,  0);
	glEnd();
	glColor3d( 0.0, 1.0, 0.0);		//Y軸は緑
	glMaterialfv(GL_FRONT,GL_DIFFUSE,glgreen);
	glBegin(GL_LINES);
		glVertex3f(  0,-10,  0);
		glVertex3f(  0, 10,  0);
	glEnd();
	glColor3d( 0.0, 0.0, 2.0);		//Z軸は青
	glMaterialfv(GL_FRONT,GL_DIFFUSE,glblue);
	glBegin(GL_LINES);
		glVertex3f(  0,  0,-10);
		glVertex3f(  0,  0, 10);
	glEnd();
	glPopMatrix();
}



/*=========================================================================
【関数】initTexture
【用途】IplImageからテクスチャの取得
【引数】
		*image　IplImagのポインタ
【戻値】なし
【仕様】RGB各8bit，24bit画像が対象
=========================================================================*/

static GLubyte texture[TEX_HEIGHT][TEX_WIDTH][4];

int tex_saveflg=1;
int tex_writeflg=0;

void initTexture(IplImage *image){
    int i, j, c;

    if(tex_writeflg){
        tex_saveflg=0;
        for (i=0;i<TEX_HEIGHT;i++) {
            for (j=0;j<TEX_WIDTH;j++) {
                texture[i][j][0]= image->imageData[i * image->widthStep+ j * image->nChannels +2];
                texture[i][j][1]= image->imageData[i * image->widthStep+ j * image->nChannels + 1];
                texture[i][j][2]= image->imageData[i * image->widthStep+ j * image->nChannels + 0];
                texture[i][j][3]= 255;//frame2->imageData[i*320+3*j+3];//255;
            }
        }
        tex_saveflg=1;
    }

}



/*=========================================================================
【関数】getTexture
【用途】InitTexture使用時のデッドロック回避
【引数】なし
【戻値】なし
【仕様】TimerでgetTexture()，init()，display()を順に呼び出す
=========================================================================*/

void getTexture(){
    if(tex_saveflg){
        tex_writeflg=0;
        diff_OneShot();
        cvCopy(now,bg,NULL);
        tex_writeflg=1;
    }
}



/*=========================================================================
【関数】myBox
【用途】直方体の描画
【引数】
        x,y,z   中心から頂点への距離
【戻値】なし
【仕様】なし
=========================================================================*/

void myBox(double x,double y,double z){
	GLdouble vertex[][3]={
		{ -x, -y, -z},
		{  x, -y, -z},
		{  x,  y, -z},
		{ -x,  y, -z},
		{ -x, -y,  z},
		{  x, -y,  z},
		{  x,  y,  z},
		{ -x,  y,  z}
	};

	static int face[][4]={
		{ 0, 1, 2, 3},
		{ 1, 5, 6, 2},
		{ 5, 4, 7, 6},
		{ 4, 0, 3, 7},
		{ 4, 5, 1, 0},
		{ 3, 2, 6, 7}
	};

	static GLdouble normal[][3]={
		{ 0.0, 0.0,-1.0},
		{ 1.0, 0.0, 0.0},
		{ 0.0, 0.0, 1.0},
		{-1.0, 0.0, 0.0},
		{ 0.0,-1.0, 0.0},
		{ 0.0, 1.0, 0.0},
	};

	int i,j;
	glBegin(GL_QUADS);
	for(j=0;j<6;j++){
		glNormal3dv(normal[j]);
		for(i=3;i>=0;i--){
			glVertex3dv(vertex[face[j][i]]);
		}
	}
	glEnd();
}



/*=========================================================================
【関数】myCylinder
【用途】円筒の描画
【引数】
        radius  半径
        height  長さの半分
        sides   分割数
【戻値】なし
【仕様】なし
=========================================================================*/

void myCylinder(double radius,double height,double sides){
	double step = PI*2.0/(double)sides;
	int i = 0;
	double t,x,z;
	/*上面*/
	glNormal3d( 0.0, 1.0, 0.0);
	glBegin(GL_TRIANGLE_FAN);
		while(i < sides){
			t = step*(double)i++;
			glVertex3d(radius*sin(t),height,radius*cos(t));
		}
	glEnd();
	/*底面*/
	glNormal3d( 0.0,-1.0, 0.0);
	glBegin(GL_TRIANGLE_FAN);
		while(--i >= 0){
			t = step*(double)i;
			glVertex3d(radius*sin(t),-height,radius*cos(t));
		}
	glEnd();
	/*側面*/
	glNormal3d( 0.0,-1.0, 0.0);
	glBegin(GL_QUAD_STRIP);
		while(i <= sides){
			t = step*(double)i++;
			x = sin(t);
			z = cos(t);
			glNormal3d( x, 0.0, z);
			glVertex3d(radius*x, height,radius*z);
			glVertex3d(radius*x,-height,radius*z);
		}
	glEnd();
}




////////////////////////////////////////////////////////////////
//その他

////////////////////////////////////////////////////////////////
//高精度パフォーマンスカウンタ
#define TIMER_NUMBER 8  //タイマの数
LONGLONG gt1[TIMER_NUMBER],gt2[TIMER_NUMBER];
LONGLONG gfr;           //動作クロック

/*=========================================================================
【関数】TimeMesureStart
【用途】時間計測の開始
【引数】
		PN	計測番号，複数のタイマを同時に使用できる

【戻値】なし
【仕様】TimeMesureEndまでの時間を計測する
=========================================================================*/

void TimeMesureStart(int PN){
    QueryPerformanceFrequency((LARGE_INTEGER *)&gfr);
    QueryPerformanceCounter((LARGE_INTEGER *)&gt1[PN]);
}



/*=========================================================================
【関数】TimeMesureEnd
【用途】時間計測の終了
【引数】
		PN	計測番号，複数のタイマを同時に使用できる

【戻値】計測時間
【仕様】TimeMesureStartからの時間を計測する
=========================================================================*/

double TimeMesureEnd(int PN){
    QueryPerformanceCounter((LARGE_INTEGER *)&gt2[PN]);
    double time = (double)(gt2[PN]-gt1[PN]) / gfr * 1000;
    return time;
}




#endif　//__MYCVGL__