#include "testApp.h"

//--------------------------------------------------------------
//units ~ 10cm
//
void testApp::setup(){
	
	bLearnBakground = true;
	cam_angle=0;
	threshold = 80;
	
	loadSettings("settings.xml");

	vidPlayer.loadMovie("camoutput.mov"); //footage/ camera needs to be the same res as opencv planes and output
	vidPlayer.setLoopState(OF_LOOP_NORMAL);
        vidGrabber.setVerbose(true);
        if (vidGrabber.initGrabber(640,480)) {
		hasCamera=true;
		useCamera=true;
	}
	else 
	{
		hasCamera=false;
		useCamera=false;
		vidPlayer.play();
	}


	colorImg.allocate(640,480);
	colorImg.setUseTexture(true);
	grayImage.allocate(640,480);
	grayBg.allocate(640,480);
	grayDiff.allocate(640,480);

	ofVec3f centre=ofVec3f(ofGetWidth()/2,0,0);
	ofVec3f normal=ofVec3f(0,0,-1);
	ray=ofRay();
	plane=ofPlane(centre,normal);
	plane.color=ofColor(255,255,255);

	projector=ofProjector(2.0f, ofVec2f(0.0f, 0.0f),ofGetWidth(),ofGetHeight()); //1.535f
	projector.setPosition(ofGetWidth()/2,ofGetHeight()/2,-ofGetWidth());
	projector.lookAt(ofVec3f(ofGetWidth()/2,ofGetHeight()/2,0),ofVec3f(0, -1, 0));

	cam=ofCamera();
	cam.setPosition(ofGetWidth()/2,ofGetHeight()/2,-ofGetWidth());
	cam.lookAt(ofVec3f(ofGetWidth()/2,ofGetHeight()/2,0),ofVec3f(0, -1, 0));
	cam.setFov(41.0); //39.85); //53.13);
	
	updatePlane();
	
	mode=PLAY;
	
}

ofVec2f testApp::screen2plane(ofVec2f screenpos){
	ofVec3f p;
	ray=projector.castPixel(screenpos.x,screenpos.y); 
	bool hit = plane.intersect(ray,p);
	return ofVec2f(p.x,pow(pow(p.y,2)+pow(p.z,2),0.5f));
}
ofVec3f testApp::plane2world(ofVec2f planepos){
	return ofVec3f(planepos.x,planepos.y,0);
}

void testApp::updatePlane(){
	plane.setNormal(ofVec3f(0,sin(cam_angle*0.01745329),-cos(cam_angle*0.01745329)));
	trapDoor=trapdoor(screen2plane(ofVec2f(ofGetWidth(),0)),screen2plane(ofVec2f(ofGetWidth(),ofGetHeight())),ofVec2f(20,20));
	//create ground mesh with hole for trapdoor
	ground=ofMesh();
	ground.addVertex(plane2world(screen2plane(ofVec2f(0,0))));
	ground.addTexCoord(ofVec2f(0,0));
	ground.addVertex(plane2world(screen2plane(ofVec2f(ofGetWidth(),0))));
	ground.addTexCoord(ofVec2f(1,0));
	ground.addVertex(plane2world(screen2plane(ofVec2f(ofGetWidth(),ofGetHeight()))));
	ground.addTexCoord(ofVec2f(1,1));
	ground.addVertex(plane2world(screen2plane(ofVec2f(0,ofGetHeight()))));
	ground.addTexCoord(ofVec2f(0,1));
	
	ground.addTriangle(0,2,1);
	ground.addTriangle(0,3,2);
	
	/*
	vector<ofVec2f> corners=trapDoor.getCorners();
	for (int i=0;i<corners.size();i++) {
		ground.addVertex(plane2world(corners[0]));
		ground.addTexCoord(trapDoor.bounds2UV(corners[0]));
	}
	//join a quad for each side
	for (int i=0;i<corners.size();i++) {
		ground.addTriangle(i,i+4,i+1);
		ground.addTriangle(i+4,(i+5)%ground.getNumVertices(),i+1);
	}
	*/
	
}

//--------------------------------------------------------------
void testApp::update(){
	ofBackground(100,100,100);

    bool bNewFrame = false;

	if (useCamera) {
	       vidGrabber.grabFrame();
		bNewFrame = vidGrabber.isFrameNew();
	}else {
		vidPlayer.idleMovie();
		bNewFrame = vidPlayer.isFrameNew();
	}

	if (bNewFrame){

	if (useCamera) {
            colorImg.setFromPixels(vidGrabber.getPixels(), 640,480);
	}else {
            colorImg.setFromPixels(vidPlayer.getPixels(), 640,480);
        }

        grayImage = colorImg;
		if (bLearnBakground == true){
			grayBg = grayImage;		// the = sign copys the pixels from grayImage into grayBg (operator overloading)
			bLearnBakground = false;
		}

		// take the abs value of the difference between background and incoming and then threshold:
		grayDiff.absDiff(grayBg, grayImage);
		grayDiff.threshold(threshold);

		// find contours which are between the size of 20 pixels and 1/3 the w*h pixels.
		// also, find holes is set to true so we will get interior contours as well....
		
		//hard coded size threshold of 100 pix
		contourFinder.findContours(grayDiff, 200, (640*480)/3, 20, false);	// don't find holes
	}

}

//--------------------------------------------------------------
void testApp::draw(){
	cam.begin();
	
	switch(mode) {
		case PLAY:
			glDisable(GL_DEPTH_TEST);
			//must be a better way than this
			colorImg.draw(0,0,ofGetWidth(),ofGetHeight());
			ofFill();
			ofSetHexColor(0x000000);
			ofRect(0,0,ofGetWidth(),ofGetHeight());
			ofSetHexColor(0xffffff);
			ofPushMatrix();
				ofRotate(cam_angle,1,0,0);
				bindTexture(colorImg);
				ground.draw();
				unbindTexture(colorImg);
				trapDoor.checkUpdate(players);
				trapDoor.draw();
			ofPopMatrix();
		break;
		case CALIBRATE:
			ofFill();
			ofSetHexColor(0x333333);
			ofSetHexColor(0xa0a0a0);
			// we could draw the whole contour finder
			//contourFinder.draw(360,540);
			//keep running background average- how do you use a custom background
			ofSetHexColor(0xffffff);
			colorImg.draw(0,0,ofGetWidth(),ofGetHeight());
		
			ofPushMatrix();
				ofRotate(cam_angle,1,0,0);
				for (float i=0;i<=ofGetWidth();i+=ofGetWidth()/10) {
				glBegin(GL_LINES);
				    glVertex3f(i,0,0);
				    glVertex3f(i,ofGetWidth(),0);
				glEnd();
				glBegin(GL_LINES);
				    glVertex3f(0,i,0);
				    glVertex3f(ofGetWidth(),i,0);
				glEnd();
				}
			ofPopMatrix();

			vector<ofVec3f> newPlayers;
			float movethresh=10;
			for (int i = 0; i < contourFinder.nBlobs; i++){
				//do some bounds checking, size threshold and overlap removal
				//in order to translate blobs into players
				//attempt to track players - maintain ID no
				//attempt to base blobs
				//TODO attempt to estimate bounds shape
				//project all of this into the 3D space
				ofRectangle r=contourFinder.blobs[i].boundingRect;
				ofVec2f blobBase=ofVec2f(r.x+(r.width/2),r.y+r.height-(r.width/2));
				contourFinder.blobs[i].draw(0,0);
				ofPoint p=contourFinder.blobs[i].centroid;
				ofSetHexColor(0xffff00);
				char numStr[16];
				sprintf(numStr, "%i", i);
				ofDrawBitmapString(numStr, r.x, r.y);
				ofVec3f pp;
				ray=projector.castPixel(blobBase.x,blobBase.y); //(ofGetHeight()-y));
				bool hit = plane.intersect(ray,pp);	
				for (int i=0;i<players.size();i++) {
					if (abs(players[i].x-pp.x)<10&&abs(players[i].z-pp.z)) {
						//identified a player
						//this is flawed: tracking boxes in a skewed axis
						//how to get UV coords on the plane
						//maybe rotate the camera instead of the plane
						
					}
				}
				ofPushMatrix();
				ofTranslate(pp);
				ofRotate(cam_angle,1,0,0);
				ofBox(0,-10,0,20);
				//for now, draw in this axis
				ofPopMatrix();
			}
			
			trapDoor.checkUpdate(players);
			ofPushMatrix();
				ofRotate(cam_angle,1,0,0);
				trapDoor.draw();
			ofPopMatrix();
			
			
			// finally, a report:

			ofSetHexColor(0xffffff);
			char reportStr[1024];
			sprintf(reportStr, "threshold %i\nnum blobs found %i, fps: %f", threshold, contourFinder.nBlobs, ofGetFrameRate());
			ofDrawBitmapString(reportStr, 10, ofGetHeight()-40);
		break;
	}
	cam.end();
}

//--------------------------------------------------------------
void testApp::keyPressed(int key){

	switch (key){
		case ' ':
			bLearnBakground = true;
			break;
		case '+':
			threshold ++;
			if (threshold > 255) threshold = 255;
			break;
		case '-':
			threshold --;
			if (threshold < 0) threshold = 0;
			break;
		case 'a':
			cam_angle+=1;
			updatePlane();
			break;
		case 'z':
			cam_angle-=1;
			updatePlane();
			break;
		case 's':
			saveSettings("settings.xml");
			break;
		case '1':
			mode=PLAY;
			break;
		case '2':
			mode=CALIBRATE;
			break;
	}
}

//--------------------------------------------------------------
void testApp::keyReleased(int key){

}

//--------------------------------------------------------------
void testApp::mouseMoved(int x, int y ){

}

//--------------------------------------------------------------
void testApp::mouseDragged(int x, int y, int button){

}

//--------------------------------------------------------------
void testApp::mousePressed(int x, int y, int button){

}

//--------------------------------------------------------------
void testApp::mouseReleased(int x, int y, int button){
	pos=ofVec2f(x,y);
	//if (hit) printf("ray:%i,%i hit plane:%f,%f,%f\n",x,y,pos.x,pos.y,pos.z);
	//else printf("ray:%i,%i missed plane\n",x,y);

}

//--------------------------------------------------------------
void testApp::windowResized(int w, int h){

}

//--------------------------------------------------------------
void testApp::gotMessage(ofMessage msg){

}

//--------------------------------------------------------------
void testApp::dragEvent(ofDragInfo dragInfo){

}

void testApp::loadSettings(string filename){
	if( !XML.loadFile(filename) ){
		printf("unable to load %s check data/ folder\n",filename.c_str());
	}else{
		cam_angle=ofToInt(XML.getAttribute("gauntlet","cam_angle","none",0));
		threshold=ofToInt(XML.getAttribute("gauntlet","threshold","none",0));
		printf("loaded %s\n",filename.c_str());
	}
}
//--------------------------------------------------------------
void testApp::saveSettings(string filename){
	XML.setAttribute("gauntlet","cam_angle",ofToString(cam_angle),0);
	XML.setAttribute("gauntlet","threshold",ofToString(threshold),0);
	XML.saveFile(filename);
	printf("saved %s\n",filename.c_str());
}