#include "ofApp.h"

/*
so far so good, NOW

threading

camera + button
transitions

start with a max size say 64 - should be 16k inc mip maps

graphics card has 2048MB
9% in use by ubuntu
117964 textures
I wonder is this faster with shared memory..

performance?

use opencv to convert to floats? its a quick one..

*/
//--------------------------------------------------------------
void ofApp::setup() {
	ofSetLogLevel(OF_LOG_WARNING);

	ofSetFrameRate(60);

	kinect.setRegistration(true);
	kinect.init();
	kinect.open();
	//colourImage.allocate(kinect.width, kinect.height);
	//depthImage.allocate(kinect.width, kinect.height);
	//floatImage.setNativeScale(0.0f,255.0f);
	int h=ceil(ofGetHeight()/MAX_TILE_SIZE);
	//int w=(h*4)/3;
	//floatImage.allocate(w*FLANN_MATRIX_SIZE,h*FLANN_MATRIX_SIZE);
	floatImage=new float[h*FLANN_MATRIX_SIZE*h*FLANN_MATRIX_SIZE*3];
	//cerr<<"floatImage: "<<h<<"*"<<FLANN_MATRIX_SIZE<<"*"<<h<<"*"<<FLANN_MATRIX_SIZE<<"*3"<<endl;

	farThreshold = 70;
	angle = 0;
	kinect.setCameraTiltAngle(angle);
	kinect.enableDepthNearValueWhite(true);
	kinect.setDepthClipping(0,4000);

	store.start();

	mode=MODE_COLOURTILES;

	fullscreen=false;

	tiledata=new float[FLANN_MATRIX_SIZE*FLANN_MATRIX_SIZE*3];
	frametiledata=new float[FLANN_MATRIX_SIZE*FLANN_MATRIX_SIZE*3*h*h];
}

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

	ofSetWindowTitle(ofToString(ofGetFrameRate()));

	kinect.update();

	// there is a new frame and we are connected
	if(kinect.isFrameNew()) {

		depthImage.setFromPixels(kinect.getDepthPixels(), kinect.width, kinect.height);
		colourImage.setFromPixels(kinect.getPixels(), kinect.width, kinect.height);

		depthImage.threshold(farThreshold);

		//threshold needs to be multiplied by the original
		depthImage2.setFromPixels(kinect.getDepthPixels(), kinect.width, kinect.height);
		depthImage*=depthImage2;

		//depthImage.extend(extend_w,extend_h);
		//colourImage.extend(extend_w,extend_h);

		int h=ceil(ofGetHeight()/MAX_TILE_SIZE);
		int w=(h*4)/3;
		depthImage.resize(w,h);
		

		int fh=h*FLANN_MATRIX_SIZE;
		int fw=w*FLANN_MATRIX_SIZE;
		colourImage.resize(fw,fh);

		//cerr<<"needed: "<<h<<"*"<<FLANN_MATRIX_SIZE<<"*"<<h<<"*"<<FLANN_MATRIX_SIZE<<"*3"<<endl;


		int kinexstart=(colourImage.getWidth()/2)+(colourImage.getHeight()/2);
		uint8_t* cptr=colourImage.getPixels();
		for (int i=0;i<fh;i++){
			for (int j=0;j<fh;j++){
				floatImage[(i*fh+j)*3]=cptr[(((i*fw)+kinexstart)-j)*3];
				floatImage[(i*fh+j)*3+1]=cptr[(((i*fw)+kinexstart)-j)*3+1];
				floatImage[(i*fh+j)*3+2]=cptr[(((i*fw)+kinexstart)-j)*3+2];
			}
		}
/*
		store.flann(cptr,w,h);

		int tilestep=(FLANN_MATRIX_SIZE*FLANN_MATRIX_SIZE*3);
		for (int i=0;i<h;i++){
			for (int j=0;j<w;j++){
				for (int k=0;k<FLANN_MATRIX_SIZE;k++){
					for (int l=0;l<FLANN_MATRIX_SIZE;l++){
						frametiledata[((k*FLANN_MATRIX_SIZE+l)*3)+(tilestep*(j*w+i))]=cptr[((i*FLANN_MATRIX_SIZE+k)*fw+kinexstart)-(j*FLANN_MATRIX_SIZE+l))*3];
						frametiledata[((k*FLANN_MATRIX_SIZE+l)*3)+(tilestep*(j*w+i))]=cptr[((i*FLANN_MATRIX_SIZE+k)*fw+kinexstart)-(j*FLANN_MATRIX_SIZE+l))*3];
						frametiledata[((k*FLANN_MATRIX_SIZE+l)*3)+(tilestep*(j*w+i))]=cptr[((i*FLANN_MATRIX_SIZE+k)*fw+kinexstart)-(j*FLANN_MATRIX_SIZE+l))*3];
					}
				}
			}
		}
*/

		//floatImage.allocate(w*FLANN_MATRIX_SIZE,h*FLANN_MATRIX_SIZE);
		//floatImage=colourImage;
		//floatImage.scaleIntoMe(colourImage);

		// mark pixels and texture dirty
		depthImage.flagImageChanged();
		//colourImage.flagImageChanged();
		//floatImage.flagImageChanged();

		/*
		ofxCvColorImage *prevCol=&colourImage;
		ofxCvGrayscaleImage *prevDepth=&depthImage;


		for (int i=0;i<colourTiles.size();i++){
			colourTiles[i].scaleIntoMe(*prevCol);
			depthTiles[i].scaleIntoMe(*prevDepth);
			prevCol=&colourTiles[i];
			prevDepth=&depthTiles[i];

			colourTiles[i].flagImageChanged();
			depthTiles[i].flagImageChanged();
		}
		*/
	}

	store.update();


}



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

	ofBackground(0,0,0);

	ofSetColor(255, 255, 255);

	int fw,w,xstart,ystart,kinexstart;

	switch(mode){
		case MODE_COLOURTILES:
			//store.draw();
			xstart=(ofGetWidth()/2)-((depthImage.getHeight()/2)*MAX_TILE_SIZE);
			ystart=(ofGetHeight()/2)-((depthImage.getHeight()/2)*MAX_TILE_SIZE);
			kinexstart=(colourImage.getWidth()/2)-(colourImage.getHeight()/2);
			ofPushMatrix();
				ofTranslate(xstart,ystart);
				w=colourImage.getWidth();
				fw=colourImage.getHeight();
				for (int i=0;i<depthImage.getHeight();i++){
					for (int j=0;j<depthImage.getHeight();j++){
						//uint8_t* ppt=&colourImage.getPixels()[((j*FLANN_MATRIX_SIZE)*w+(w-((i*FLANN_MATRIX_SIZE)+kinexstart)))*3];

						for (int k=0;k<FLANN_MATRIX_SIZE;k++){
							for (int l=0;l<FLANN_MATRIX_SIZE;l++){
								
								//tiledata[(l*FLANN_MATRIX_SIZE+k)*3]=colourImage.getPixels()[((j*FLANN_MATRIX_SIZE+l)*w+(w-((i*FLANN_MATRIX_SIZE+k)+kinexstart)))*3];
								//tiledata[(l*FLANN_MATRIX_SIZE+k)*3+1]=colourImage.getPixels()[((j*FLANN_MATRIX_SIZE+l)*w+(w-((i*FLANN_MATRIX_SIZE+k)+kinexstart)))*3+1];
								//tiledata[(l*FLANN_MATRIX_SIZE+k)*3+2]=colourImage.getPixels()[((j*FLANN_MATRIX_SIZE+l)*w+(w-((i*FLANN_MATRIX_SIZE+k)+kinexstart)))*3+2];
								
								tiledata[(l*FLANN_MATRIX_SIZE+k)*3]=floatImage[(((j*FLANN_MATRIX_SIZE+l)*fw)+(i*FLANN_MATRIX_SIZE)+k)*3];
								tiledata[(l*FLANN_MATRIX_SIZE+k)*3+1]=floatImage[(((j*FLANN_MATRIX_SIZE+l)*fw)+(i*FLANN_MATRIX_SIZE)+k)*3+1];
								tiledata[(l*FLANN_MATRIX_SIZE+k)*3+2]=floatImage[(((j*FLANN_MATRIX_SIZE+l)*fw)+(i*FLANN_MATRIX_SIZE)+k)*3+2];
								
							}
						}

						//ofImage& im=store.get_image(ofColor(ppt[0],ppt[1],ppt[2]));
						//ofSetColor(ppt[0],ppt[1],ppt[2]);
						//ofRect(i*MAX_TILE_SIZE,j*MAX_TILE_SIZE,MAX_TILE_SIZE,MAX_TILE_SIZE);
						//ofImage& im=store.get_image(ppt[0],ppt[1],ppt[2]);
						ofImage& im=store.get_image(tiledata);
						if (im.isUsingTexture()) im.draw(i*MAX_TILE_SIZE,j*MAX_TILE_SIZE);
					}
				}
			ofPopMatrix();
			break;

		case MODE_DEPTH:

			depthImage.draw(0,0,ofGetWidth(),ofGetHeight());
			break;

		case MODE_COMPONENTS:

			store.draw();
			//depthImage.draw(0,0, 640,480);
			//colourImage.draw(640,0, 640,480);
			break;
	}

}



//--------------------------------------------------------------
void ofApp::exit() {
	delete tiledata;
	delete floatImage;
	kinect.setCameraTiltAngle(0); // zero the tilt on exit
	kinect.close();
}

//--------------------------------------------------------------
void ofApp::keyPressed (int key) {
	switch (key) {

		case '>':
		case '.':
			farThreshold ++;
			if (farThreshold > 255) farThreshold = 255;
			break;

		case '<':
		case ',':
			farThreshold --;
			if (farThreshold < 0) farThreshold = 0;
			break;


		case 'w':
			kinect.enableDepthNearValueWhite(!kinect.isDepthNearValueWhite());
			break;

		case 'o':
			kinect.setCameraTiltAngle(angle); // go back to prev tilt
			kinect.open();
			break;

		case 'c':
			kinect.setCameraTiltAngle(0); // zero the tilt
			kinect.close();
			break;

		case '1':
			kinect.setLed(ofxKinect::LED_GREEN);
			break;

		case '2':
			kinect.setLed(ofxKinect::LED_YELLOW);
			break;

		case '3':
			kinect.setLed(ofxKinect::LED_RED);
			break;

		case '4':
			kinect.setLed(ofxKinect::LED_BLINK_GREEN);
			break;

		case '5':
			kinect.setLed(ofxKinect::LED_BLINK_YELLOW_RED);
			break;

		case '0':
			kinect.setLed(ofxKinect::LED_OFF);
			break;

		case OF_KEY_UP:
			angle++;
			if(angle>30) angle=30;
			kinect.setCameraTiltAngle(angle);
			break;

		case OF_KEY_DOWN:
			angle--;
			if(angle<-30) angle=-30;
			kinect.setCameraTiltAngle(angle);
			break;

		case OF_KEY_LEFT:
			mode--;
			if (mode<0) mode=NUM_MODES-1;
			break;

		case OF_KEY_RIGHT:
			mode=(mode+1)%NUM_MODES;
			break;

		case ' ':
			fullscreen=!fullscreen;
			ofSetFullscreen(fullscreen);
			break;

	}
}

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

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

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

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