#include "testApp.h"

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

    ofBackground(255,255,255);

	#ifdef _USE_LIVE_VIDEO
        vidGrabber.initGrabber(320,240); //720x576 BBC Screen
	#else
        vidPlayer.loadMovie("city02.mov");
        vidPlayer.play();
        vidPlayer.setVolume(0);
	#endif

    colorImg.allocate(320,240);
    gauss_foregroundImg.allocate(320,240);
    gauss_backgroundImg.allocate(320,240);

    // not needed unless bgModel is used instead of gauss_bgModel below
    CvGaussBGStatModelParams* params = new CvGaussBGStatModelParams;
    params->win_size=100;//2;
    params->n_gauss=5;//5;
    params->bg_threshold=0.7;//0.7;
    params->std_threshold=2.5;//3.5;
    params->minArea=15;
    params->weight_init=0.05;
    params->variance_init=30;

    /* default parameters of gaussian background detection algorithm */
    //BACKGROUND_THRESHOLD     0.7     /* threshold sum of weights for background test */
    //STD_THRESHOLD            2.5     /* lambda=2.5 is 99% */
    //WINDOW_SIZE              200     /* Learning rate; alpha = 1/CV_GBG_WINDOW_SIZE */
    //NGAUSSIANS               5       /* = K = number of Gaussians in mixture */
    //WEIGHT_INIT              0.05
    //SIGMA_INIT               30
    //MINAREA                  15.f

    gauss_bgModel = cvCreateGaussianBGModel(colorImg.getCvImage() ,params);
    //gauss_bgModel = cvCreateGaussianBGModel(colorImg.getCvImage()); // WORKS WELL

    myFont.loadFont("gothic.ttf", 10);

    // PIXELS
    location = 0;
    pic.loadImage("flower.jpg");
    pixels = pic.getPixels();

    // DRAWING
    nPts = 0;

    // BALLS
	ofSetVerticalSync(true);
	ofSetCircleResolution( 100 );

	box2d.init();
	box2d.setGravity( 0, 9.8 );
	box2d.createBounds();
	box2d.registerGrabbing();
	box2d.getWorld()->SetContactListener( &contacts );	// register contact class

	for( int i=0; i<20; i++ ) { // number of balls in main window
		balls.push_back( CustomBall() );
		CustomBall& circle = balls.back();

		float mass		= 3.0;
		float bounce	= 0.53;
		float friction	= 0.1;
		float radius	= 10.0;

		circle.setPhysics( mass, bounce, friction );
		circle.setup( box2d.getWorld(), ofRandom( 0, ofGetWidth() ), ofRandom( 0, ofGetHeight() ), radius, false );
		circle.setVelocity( ofRandom( -10, 10 ), ofRandom( -10, 10 ) );

		circle.r = 255; circle.g = 255; circle.b = 255;
	}

	// COLLISION
	simpleAmount     = 1.0;//1.0;
	threshold		 = 152;

	colorCollision.allocate(320, 240);
	//grayCollision.allocate(320, 240);
	contourAnalysis.setSize(320, 240);

	box2dCollision.init();
	box2dCollision.setGravity(0, 20);//(0, 20);
	box2dCollision.registerGrabbing();
	box2dCollision.createBounds(20, 320, 320, 240);//(20, 300, 320, 240);
	box2dCollision.setFPS(30.0);

	// SAVE PNG IMAGES FOR VIDEO
	//saved_image_num = 0;
}

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

    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);
            colorCollision.setFromPixels(vidGrabber.getPixels(), 320,240);//
	    #else
            colorImg.setFromPixels(vidPlayer.getPixels(), 320,240);
            colorCollision.setFromPixels(vidPlayer.getPixels(), 320,240);//
        #endif

        // Update models
        cvUpdateBGStatModel(colorImg.getCvImage() ,gauss_bgModel);
        gauss_foregroundImg = gauss_bgModel->foreground;
        gauss_backgroundImg = gauss_bgModel->background;

        // 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(gauss_foregroundImg, 20, (340*240)/3, 10, false); // find holes

		// COLLISION
        //grayCollision = colorCollision;
		//grayCollision.threshold(threshold, true);
		//contour.findContours(grayCollision, 30, 320*240, 20, true);
		contour.findContours(gauss_foregroundImg, 20, (320*240)/3, 10, false);//////

		//if(contour.blobs.size() > 0) {
		//	contourAnalysis.simplify(contour.blobs[0].pts, simpleCountour, simpleAmount);
		if(contourFinder.blobs.size() > 0) {
		    //for (int i=contourFinder.blobs.size();i>0; i--) {
            contourAnalysis.simplify(contourFinder.blobs[0].pts, simpleCountour, simpleAmount);
		    //}
			//contourAnalysis.simplify(contourFinder.blobs[0].pts, simpleCountour, simpleAmount);
            //simpleCountour = contourFinder.blobs[0].pts;
			ofPoint p;
			lineStrip.setWorld(box2dCollision.getWorld());
			lineStrip.clear();

			// we need to loop backward to make the top
			// of the contour the active side else go the other way.
            for(int i=simpleCountour.size()-1; i>0; i--) {
                p.x = simpleCountour[i].x + 20;
                p.y = simpleCountour[i].y + 320;
                lineStrip.addPoint(p.x, p.y);
            }
			lineStrip.createShape();
		}
    }
    // COLLISON
    box2dCollision.update();
    // BALLS
	box2d.update();
	for( int i=0; i<balls.size(); i++ ){
		balls[ i ].update();
	}

    /*
	// GRAB PNG OF EACH FRAME OF SCREEN FOR VIDEO
    static char file_name[255];
    sprintf(file_name, "output/output_%0.4i.png", saved_image_num);
    img_saver.grabScreen(0, 0, ofGetWidth(), ofGetHeight());
    printf("Saving file: %s\n", file_name);
    img_saver.saveImage(file_name);
    saved_image_num++;
    */
}

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

   ofSetColor(0xffffff);
   colorImg.draw(20,40);

   gauss_foregroundImg.draw(360,40); //moving people
   gauss_backgroundImg.draw(700,40); //still background

   	// contours
	ofFill();
	ofSetColor(0x333333);
	ofRect(360,320,320,240);
	ofSetColor(0xffffff);
	contourFinder.draw(360,320);

    // TEXT
    ofSetColor(0x000000);
    myFont.drawString("Original", 20,300);
	myFont.drawString("Foreground", 360,300);
	myFont.drawString("Background", 700,300);
	myFont.drawString("Collision", 20,580);
    myFont.drawString("Contours", 360,580);
    myFont.drawString("Pixels", 700,580);
    myFont.drawString("Draw something! (Right click to clear...)", 20,720);

	char info[255]; char info2[255]; char info3[255]; char info4[255];
    //sprintf(info, "\nThreshold: ", threshold);
    sprintf(info2, "\nSimple Amount [left/right]: %f", simpleAmount);
    sprintf(info3, "\n\nPress [space] to add big balls.");
    sprintf(info4, "\nPress [b] to add small balls.");
    myFont.drawString(info2, 20,590);
    myFont.drawString(info3, 20,595);
    myFont.drawString(info4, 20,600);

    // PIXELS
    //char buffer[50];
    //int n;
    //n=sprintf(buffer, "%d %d", mouseX, mouseY);
    //myFont.drawString(buffer, 20,560);
    ofSetColor(0xffffff);
    pic.draw(700,320);

    // only draw the cirle with the image
    if ((mouseX >= 700) && (mouseX <= 700 + pic.getWidth()) &&
        (mouseY >= 320) && (mouseY <= 320 + pic.getHeight())) {

        //location of the mouse, where the circle is to be drawn
        location = ((mouseY-320) * pic.width) + (mouseX-700);
        int r = pixels[3 * location];
        int g = pixels[3 * location+1];
        int b = pixels[3 * location+2];
        ofSetColor(r, g, b);
        ofFill();
        ofCircle(mouseX, mouseY, 20);
        ofNoFill();
        ofSetColor(0xffffff);
    }

    // COLLISION
    ofSetColor(0xffffff);
	colorCollision.draw(20, 320); ////
    //gauss_foregroundImg.draw(20,300); ///////

	ofNoFill();
	ofSetColor(0x00ff6a); //tuerkis
	//contour.draw(20, 300, 320, 240);
	contourAnalysis.draw(simpleCountour, 20, 320, 320, 240);

	// draw the box2dCollision balls
	for(int i=0; i<customParticles.size(); i++) {
		customParticles[i].draw();
	}
	ofSetLineWidth(4);
	lineStrip.draw(); // ??? blueish
    ofSetLineWidth(1);

    // BALLS
	ofSetBackgroundAuto(true);
	//-- draw balls with soft edges.
	ofFill();
	for( int i=0; i<balls.size(); i++ ) {
		balls[ i ].draw();
    }
	ofNoFill();
	ofEnableSmoothing();
	for( int i=0; i<balls.size(); i++ ) {
		balls[ i ].draw();
    }
	ofDisableSmoothing();

    // DRAWING
     ofSetColor(0xff0000);
     ofNoFill();
     ofSetLineWidth(3);
     ofBeginShape();
     for (int i = 0; i < nPts; i++){
         if (pts[i].x ==-1 && pts[i].y==-1){
             ofEndShape();
             ofBeginShape();
         } else {
             ofVertex(pts[i].x, pts[i].y);
         }
     }
     ofEndShape();
     ofSetLineWidth(1);
}

//--------------------------------------------------------------
void testApp::keyPressed  (int key){
    // gray threshold
	//if(key == OF_KEY_UP) {
	//	threshold ++;
	//	if(threshold > 255) threshold = 255;
	//}
	//if(key == OF_KEY_DOWN) {
	//	threshold --;
	//	if(threshold < 0) threshold = 0;
	//}

	// the simple amount
	if(key == OF_KEY_LEFT) {
		simpleAmount += 1.0;
		if(simpleAmount > 100.0) simpleAmount = 100.0;
	}
	if(key == OF_KEY_RIGHT) {
		simpleAmount -= 1.0;
		if(simpleAmount < 1.0) simpleAmount = 1.0;
	}

	// toggle the image draw mode
	//if(key == '1') bDrawGray = !bDrawGray;

	// big balls
	if(key == ' ') {
		float r = ofRandom(5, 15);
		CustomParticle p;
		// Mass Bounce Friction
		p.setPhysics(0.0001, 0.79, 0.01);
		p.setup(box2dCollision.getWorld(), mouseX, mouseY, r);
		p.c = (int)ofRandom(30, 100);
		customParticles.push_back(p);
	}

	// small balls
	if(key == 'b') {
		float r = ofRandom(3, 8);
		CustomParticle p;
		// Mass Bounce Friction
		p.setPhysics(0.0001, 0.73, 0.9);
		p.setup(box2dCollision.getWorld(), mouseX, mouseY, r);
		p.c = (int)ofRandom(100, 255); //(3, 255);
		customParticles.push_back(p);
	}
}

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

//--------------------------------------------------------------
void testApp::mouseDragged(int x, int y, int button){
     if (nPts < MAX_N_PTS){
        pts[nPts].x = x;
        pts[nPts].y = y;
        nPts++;
     }
}

//--------------------------------------------------------------
void testApp::mousePressed(int x, int y, int button){
    if (button==2){ // 2 = Right Mouse Button, 1 = Center
        nPts = 0;
    }
}

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

    if (nPts < MAX_N_PTS){
        pts[nPts].x = -1;
        pts[nPts].y = -1;
        nPts++;
     }
}

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

void MyContactListener :: Add( const b2ContactPoint* point ) {
	testApp* appPtr = ((testApp*)ofGetAppPtr());
	appPtr->box2dContactEventHandler( point );
}

void MyContactListener :: Remove(const b2ContactPoint* point) {
	//
}

// BALLS
void testApp :: box2dContactEventHandler ( const b2ContactPoint* point ) {
    // BALLS
	for( int i=0; i<balls.size(); i++ ) {
		CustomBall& ball = balls[ i ];
		for( b2Shape* s=ball.body->GetShapeList(); s; s=s->GetNext() ) {
			if( point->shape1 == s || point->shape2 == s ) {
				ball.r = 0; // turn green on contact
				ball.b = 0;
				ball.g = 255;
			}
		}
	}
}

void CustomBall :: update () {
	r += ( 255 - r ) * 0.1; // fade back pink after contact
	g += ( 0 - g ) * 0.1;
	b += ( 255 - b ) * 0.1;
}
// BALLS
void CustomBall :: draw () {
	ofSetColor( r, g, b );
	ofCircle( getPosition().x, getPosition().y, getRadius() );
}
