//作成者 前田直哉　制作日 3/12

#include "main.h"
#include "keyboard.h"
#include "callBack.h"

void glInt(int ,char*);
void idle();
void display();
void drawJointPoint(xn::DepthGenerator&, xn::SkeletonCapability&, XnSkeletonJoint, std::string, XnUserID, Mat_<Vec3b>);

void glInit (int argc, char *argv[]){
 glutInit(&argc, argv);
 glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);
 glutInitWindowSize(640, 480);
 glutCreateWindow("sample");
 glDisable(GL_DEPTH_TEST);
 glEnable(GL_TEXTURE_2D);

 glEnableClientState(GL_VERTEX_ARRAY);
 glDisableClientState(GL_COLOR_ARRAY);

 CvMat* Amatrix = cvCreateMat(3,3,CV_32FC1);

 cvmSet(Amatrix, 0, 0, 526.37);
 cvmSet(Amatrix, 0, 1, 0.0);
 cvmSet(Amatrix, 0, 2, 313.68);
 cvmSet(Amatrix, 1, 0, 0.0);
 cvmSet(Amatrix, 1, 1, 526.37);
 cvmSet(Amatrix, 1, 2, 259.01);
 cvmSet(Amatrix, 2, 0, 0.0);
 cvmSet(Amatrix, 2, 1, 0.0);
 cvmSet(Amatrix, 2, 2, 1.0);


 glClearColor(0.0, 0.0, 0.0, 1.0);
 int camW = 640;
 int camH = 480;
 int x0 = (int)(cvmGet(Amatrix, 0, 2) - camW / 2);
 int y0 = (int)(camH / 2 - cvmGet(Amatrix, 1, 2));
 glViewport(x0, y0, camW, camH);
 double fovy = 2.0 * atan(camH / (2.0 * cvmGet(Amatrix, 1, 1))) * 180.0 / CV_PI;
 double aspect = (camW * cvmGet(Amatrix, 1, 1)) / (camH * cvmGet(Amatrix, 0, 0));
 glMatrixMode(GL_PROJECTION);
 glLoadIdentity();
 gluPerspective(fovy, aspect, 10, 10000);
 glMatrixMode(GL_MODELVIEW);
}


void idle(){
    glutPostRedisplay();
}


class Kinect {
protected:
	int WIDTH;
	int HEIGHT;
	xn::ImageGenerator image; // イメージコンテキスト  
	xn::DepthGenerator depth; // 深度コンテキスト
	xn::UserGenerator  user;  // ユーザコンテキスト
	xn::ImageMetaData imageMD;  //カラー画像を入れるもの
    xn::DepthMetaData depthMD;  //デプス画像を入れるもの
	xn::SceneMetaData sceneMD;  //ユーザ領域を入れるもの
	
	xn::SkeletonCapability skeleton; // トラッキングするスケルトンをいれるもの
	
	xn::Context context;

public:
	
	//コンストラクタ　主に初期化
	Kinect(): skeleton(skeleton){
		//OpenNIのもろもろの初期化
		XnCallbackHandle userCallbacks, calibrationCallbacks, poseCallbacks;
		XnChar pose[20] = "";

		context.Init();
		image.Create(context);
		depth.Create(context);

		 // ユーザーの作成
        user.Create(context);
		
		//スケルトントラッキングの生成
		skeleton = user.GetSkeletonCap();
		skeleton.GetCalibrationPose(pose);

		//何かを検出するたびに呼ばれるコールバック関数
		xn::PoseDetectionCapability Pose = user.GetPoseDetectionCap();
		Pose.RegisterToPoseCallbacks(&::PoseDetected, &::PoseLost, &user, poseCallbacks);
		user.RegisterUserCallbacks(&::UserDetected, &::UserLost, 0, userCallbacks);
		skeleton.RegisterCalibrationCallbacks(&::CalibrationStart, &::CalibrationEnd, &user, calibrationCallbacks); 
		//取得する部位．今回は全部．
		skeleton.SetSkeletonProfile(XN_SKEL_PROFILE_ALL);


		//ジェスチャー検出の開始
		context.StartGeneratingAll();
		WIDTH = 480;
		HEIGHT = 640;
	}
	~Kinect(){
		context.Shutdown();
	}

	void run(){
		glutDisplayFunc(display);
		glutIdleFunc(idle);
		glutMainLoop();
	}
	//データの更新
	void update(){
		context.WaitAndUpdateAll();
	}
	//RGB画像の取得
	Mat getRGBImage(){
		Mat_<Vec3b> rgb_image(480, 640);
		image.GetMetaData(imageMD);
		memcpy(rgb_image.data,imageMD.Data(),rgb_image.step * rgb_image.rows);
		cvtColor(rgb_image,rgb_image,CV_RGB2BGR); 
		return rgb_image;
	}
	//デプス画像の取得
	Mat getDepthImage(){
		Mat_<Vec3b> depth_image(480, 640);
		depth.GetAlternativeViewPointCap().SetViewPoint(image);					//ズレを補正
		depth.GetMetaData(depthMD);
		for(int i = 0; i < depth_image.rows;i++){
			 for (int j = 0; j < depth_image.cols; j++){
				 depth_image[i][j].val[0] = depthMD(j,i) * 255.0 / 4096.0;
				 depth_image[i][j].val[1] = 0;
				 depth_image[i][j].val[2] = depthMD(j,i) * 255.0 / 4096.0;
			 }
		}
		return depth_image;
	}
	//人の検出
	Mat getUserTracking(Mat_<Vec3b> Image){
		
		user.GetUserPixels(0, sceneMD);

		for ( int y = 0; y < WIDTH; y++){
			for ( int x = 0; x < HEIGHT; x++){
				XnLabel label = sceneMD(x,y);
				if(label > 0){
					Image[y][x].val[0] = label%2 == 0 ? 0 : 255;
					Image[y][x].val[1] = label%2 == 0 ? 255 : 0;
					Image[y][x].val[2] = label%3 == 0 ? 255 : 0;
				}
			}
		}
		return Image;
	}
	
	
	
	//人の部位の検出
	Mat getSkeletonTracking(Mat_<Vec3b> Image){
		
		user.GetUserPixels(0, sceneMD);

		for ( int y = 0; y < WIDTH; y++){
			for ( int x = 0; x < HEIGHT; x++){
				XnLabel label = sceneMD(x,y);
				if(label > 0){
					Image[y][x].val[0] = label%2 == 0 ? 0 : 255;
					Image[y][x].val[1] = label%2 == 0 ? 255 : 0;
					Image[y][x].val[2] = label%3 == 0 ? 255 : 0;
				}
			}
		}

		XnUserID allUsers[20];
		XnUInt16 nUsers = 20;
		user.GetUsers(allUsers, nUsers);

		for (int i = 0; i < nUsers; i++) {

				// キャリブレーションに成功しているかどうか
				if (skeleton.IsTracking(allUsers[i])) {

					XnUserID u = allUsers[i];
					drawJointPoint(depth, skeleton, XN_SKEL_HEAD, "HEAD", u, Image);
					//drawJointPoint(depth, skeleton, XN_SKEL_NECK, "NECK", u, Image);
					//drawJointPoint(depth, skeleton, XN_SKEL_TORSO, "TORSO", u, Image);
					//drawJointPoint(depth, skeleton, XN_SKEL_LEFT_ELBOW, "LEFT ELBOW", u, Image);
					//drawJointPoint(depth, skeleton, XN_SKEL_RIGHT_ELBOW, "RIGHT ELBOW", u, Image);
					//drawJointPoint(depth, skeleton, XN_SKEL_LEFT_HIP, "LEFT HIP", u, Image);
					//drawJointPoint(depth, skeleton, XN_SKEL_RIGHT_HIP, "RIGHT HIP", u, Image);
					//drawJointPoint(depth, skeleton, XN_SKEL_LEFT_KNEE, "LEFT KNEE", u, Image);
					//drawJointPoint(depth, skeleton, XN_SKEL_RIGHT_KNEE, "RIGHT KNEE", u, Image);
					drawJointPoint(depth, skeleton, XN_SKEL_LEFT_HAND, "LEFT HAND", u, Image);
					drawJointPoint(depth, skeleton, XN_SKEL_RIGHT_HAND, "RIGHT HAND", u, Image);
					//drawJointPoint(depth, skeleton, XN_SKEL_LEFT_SHOULDER, "SHOULDER", u, Image);
					//drawJointPoint(depth, skeleton, XN_SKEL_RIGHT_SHOULDER, "SHOULDER", u, Image);
					//drawJointPoint(depth, skeleton, XN_SKEL_LEFT_FOOT, "LEFT FOOT", u, Image);
					//drawJointPoint(depth, skeleton, XN_SKEL_RIGHT_FOOT, "RIGHT FOOT", u, Image);

					// 最初の一人だけ処理したら抜ける
					break;
				}
			}


		return Image;
	}

};

//グローバル変数---------------------

int pushKey;
Mat_<Vec3b> rgbImage(480, 640);
Mat_<Vec3b> depthImage(480, 640);
Mat_<Vec3b> userTrack(480, 640);
Mat_<Vec3b> skeletonTrack(480, 640);

Kinect kinect;

//-----------------------------------

int main(int argc, char *argv[]){
	glInit(argc,argv);
	kinect.run();
	return 0;
}

//画像上にトラックしてる部位を描画
void drawJointPoint(xn::DepthGenerator& depth, xn::SkeletonCapability& capability, XnSkeletonJoint joint, std::string name, XnUserID user, Mat_<Vec3b> Image) {
	XnSkeletonJointPosition pos;
	// ジョイント座標の取得
	capability.GetSkeletonJointPosition(user, joint, pos);
	XnPoint3D pReal[1] = {pos.position};
	XnPoint3D pProjective[1];

	//cout << name << " : (" << (double)pReal[0].X << ", " << (double)pReal[0].Y << ", " << (double)pReal[0].Z << ")" << endl;

	// 世界座標系から表示座標系に変換した座標を取得
	depth.ConvertRealWorldToProjective(1, pReal, pProjective);

	// ジョイントの位置に円を描画
	cv::circle(Image, cv::Point(pProjective[0].X, pProjective[0].Y), 6, CV_RGB(255,255,255), 2, CV_AA, 0);
	// ジョイントの名前を描画
	cv::putText(Image, name.c_str(), cv::Point(pProjective[0].X+10, pProjective[0].Y+10), cv::FONT_HERSHEY_SIMPLEX,1.0, CV_RGB(255, 230, 50),2,CV_AA);
}


//描画を更新している部分　主にここに処理を書いていくといいよ！
void display(){

	kinect.update();

	//RGB画像を取得
	rgbImage = kinect.getRGBImage();
	//デプス画像を取得
	depthImage = kinect.getDepthImage();
	//ユーザを取得
	userTrack = kinect.getUserTracking(rgbImage);
	//キャリブレーションを行った人の体の部位のトラッキング
	skeletonTrack = kinect.getSkeletonTracking(rgbImage);

	imshow("rgbImage",rgbImage);
	imshow("depthImage",depthImage);
	imshow("userTracking",userTrack);
	imshow("skeletonTracking",skeletonTrack);
	


	//キー操作部分
	pushKey = keyboard();
	if(pushKey==1){
		exit(0);
	}
}