#include "testApp.h"

//--------------------------------------------------------------
void testApp::setup(){

	#ifdef _USE_LIVE_VIDEO
        vidGrabber.setVerbose(true);
        vidGrabber.initGrabber(320,240);
	#else
        vidPlayer.loadMovie("camoutput.mov");
        vidPlayer.play();
	#endif

    colorImg.allocate(320,240);
	grayImage.allocate(320,240);
	grayBg.allocate(320,240);
	grayDiff.allocate(320,240);

	bLearnBakground = true;
	threshold = 80;

	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(1.535f, ofVec2f(0.0f, 0.5f),ofGetWidth(),ofGetHeight());
	projector.setPosition(ofGetWidth()/2,ofGetHeight()/2,ofGetHeight());

	cam=ofCamera();
	cam.setPosition(ofGetWidth()/2,ofGetHeight()/2,ofGetHeight());
	cam.lookAt(ofVec3f(ofGetWidth()/2,ofGetHeight()/2,0));
	cam.setFov(54.13);

	cam_angle=0;

}

//--------------------------------------------------------------
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(), 320,240);
        #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....
		contourFinder.findContours(grayDiff, 20, (340*240)/3, 10, 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);


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

	cam.begin();
	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();
	cam.end();

	ofSphere(pos.x,pos.y,pos.z,5);

	    for (int i = 0; i < contourFinder.nBlobs; i++){
		contourFinder.blobs[i].draw(0,0);

		ofPoint p=contourFinder.blobs[i].centroid;
		ofVec3f pp;

		cam.begin();
			ofPushMatrix();

			ofRotate(cam_angle,1,0,0);
				ray=projector.castPixel(p.x,(ofGetHeight()/2)-p.y); //(ofGetHeight()-y));
				bool hit = plane.intersect(ray,pp);
				ofBox(pp,50);
		    	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, 215);



}

//--------------------------------------------------------------
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;
	}
	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,((3*(ofGetHeight()/2))-y)); //(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){

}