#include "testApp.h"

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

	#ifdef _USE_LIVE_VIDEO
        vidGrabber.setVerbose(true);
        vidGrabber.initGrabber(320,240);
	#else
        vidPlayer.loadMovie("camoutput.wmv"); //footage/ camera needs to be the same res as opencv planes and output
        vidPlayer.play();
	#endif

	colorImg.allocate(640,480);
	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(42.0); //39.85); //53.13);
	
	trapDoor=trapdoor(ofRectangle(0,ofGetHeight()/4,ofGetHeight(),ofGetHeight()),ofVec2f(20,20));
	
	
	
}

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

    bool bNewFrame = false;

	#ifdef _USE_LIVE_VIDEO
       vidGrabber.grabFrame();
	   bNewFrame = vidGrabber.isFrameNew();
    #else
        vidPlayer.idleMovie();
        bNewFrame = vidPlayer.isFrameNew();
	#endif

	if (bNewFrame){

		#ifdef _USE_LIVE_VIDEO
            colorImg.setFromPixels(vidGrabber.getPixels(), 320,240);
	    #else
            colorImg.setFromPixels(vidPlayer.getPixels(), 640,480);
        #endif

        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(){

	/*
	// draw the incoming, the grayscale, the bg and the thresholded difference
	ofSetHexColor(0xffffff);
	colorImg.draw(20,20);
	grayImage.draw(360,20);
	grayBg.draw(20,280);
	grayDiff.draw(360,280);

	// then draw the contours:
 */
	ofFill();
	ofSetHexColor(0x333333);
	//ofRect(360,540,320,240);
	ofSetHexColor(0xa0a0a0);

	// we could draw the whole contour finder
	//contourFinder.draw(360,540);
	
	//keep running background average- how do you use a custom background


	colorImg.draw(0,0,ofGetWidth(),ofGetHeight());

	cam.begin();
	ofPushMatrix();

	ofRotate(cam_angle,1,0,0);
	//cam.Rotate(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();
	

	//ofSphere(pos.x,pos.y,pos.z,5);
	
	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);
	//for now, draw in this axis
	trapDoor.draw();
	ofPopMatrix();
	    
	cam.end();



	// 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);
}

//--------------------------------------------------------------
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;
			break;
		case 'z':
			cam_angle-=1;
			break;
		case 's':
			saveSettings("settings.xml");
			break;
	}
	plane.setNormal(ofVec3f(0,sin(cam_angle*0.01745329),-cos(cam_angle*0.01745329)));
}

//--------------------------------------------------------------
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){
	ray=projector.castPixel(x,y); //+(ofGetHeight()/2)); //(ofGetHeight()-y));
	bool hit = plane.intersect(ray,pos);
	//pos=ofVec3f(pos.x*2,pos.y*2,pos.z*2);
	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());
}