#include "ofApp.h"


//--------------------------------------------------------------
void ofApp::setup() {
	ofSetLogLevel(OF_LOG_WARNING);
	
	// enable depth->video image calibration
	kinect.setRegistration(true);
    
	kinect.init();
	//kinect.init(true); // shows infrared instead of RGB video image
	//kinect.init(false, false); // disable video image (faster fps)
	
	kinect.open();		// opens first available kinect
	//kinect.open(1);	// open a kinect by id, starting with 0 (sorted by serial # lexicographically))
	//kinect.open("A00362A08602047A");	// open a kinect using it's unique serial #
	
	// print the intrinsic IR sensor values
	if(kinect.isConnected()) {
		ofLogNotice() << "sensor-emitter dist: " << kinect.getSensorEmitterDistance() << "cm";
		ofLogNotice() << "sensor-camera dist:  " << kinect.getSensorCameraDistance() << "cm";
		ofLogNotice() << "zero plane pixel size: " << kinect.getZeroPlanePixelSize() << "mm";
		ofLogNotice() << "zero plane dist: " << kinect.getZeroPlaneDistance() << "mm";
	}
	
	
	colourImage.allocate(kinect.width, kinect.height);
	depthImage.allocate(kinect.width, kinect.height);
	
	farThreshold = 70;
	
	ofSetFrameRate(60);
	
	// zero the tilt on startup
	angle = 0;
	kinect.setCameraTiltAngle(angle);

	//start with the largest size square
	//get the depth, if below a certain size, subdivide and repeat

	//OR start at the smallest size, if adjacent squares are the same, work up

	//1 - create a pyramid of mip maps of depth
	//2 - work upwards

	//a find layout of largest tiles
	//b extend captured frame to this ratio
	//c mip map this down into prepared containers

	int tiles_w=ceil((float)kinect.width/(MAX_TILE_SIZE*2))*2;
	int tiles_h=ceil((float)kinect.height/(MAX_TILE_SIZE*2))*2;
	extend_w=(tiles_w)*MAX_TILE_SIZE;
	extend_h=(tiles_h)*MAX_TILE_SIZE;

	//get number of levels
	levels=0;
	for (int i=MIN_TILE_SIZE;i<=MAX_TILE_SIZE;i*=2) {
		levels++;
	}

	colourTiles.resize(levels);
	depthTiles.resize(levels);

	for (int i=MIN_TILE_SIZE,l=0;i<=MAX_TILE_SIZE;i*=2,l++) {
		colourTiles[l].allocate(extend_w/i,extend_h/i);
		depthTiles[l].allocate(extend_w/i,extend_h/i);
		cerr<<"level "<<l<<" mipmap: "<<colourTiles[l].getWidth()<<"x"<<colourTiles[l].getHeight()<<endl;
	}
	
}

//--------------------------------------------------------------
void ofApp::update() {
	
	ofBackground(100, 100, 100);
	
	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);
		
		// mark pixels and texture dirty
		depthImage.flagImageChanged();
		colourImage.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();
		}
	}
}

void ofApp::checktile(int level,int x,int y,int size){
	int levels_factor=255/levels;
	if (depthTiles[level].getPixels()[y*((int)depthTiles[level].getHeight())+x]>level*levels_factor&&level>0){
		for (int i=0;i<2;i++){
			for (int j=0;j<2;j++){
				checktile(level-1,x*2+i,y*2+j,size/2);
			}
		}
	}
	else {
		ofSetColor(depthTiles[level].getPixels()[(y*((int)depthTiles[level].getHeight())+x)*3],
			depthTiles[level].getPixels()[(y*((int)depthTiles[level].getHeight())+x)*3+1],
			depthTiles[level].getPixels()[(y*((int)depthTiles[level].getHeight())+x)*3+2]);
		ofRect(x*size,y*size,size,size);
	}
}

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

	ofSetColor(255, 255, 255);

	//recursively draw pixels

	int pixelsize=ofGetWidth()/MAX_TILE_SIZE;

	for (int i=0;i<colourTiles[levels-1].getWidth();i++){
		for (int j=0;j<colourTiles[levels-1].getHeight();j++){
			checktile(levels-1,i,j,pixelsize);
		}
	}
	
	/*

	depthImage.draw(10, 10, 640, 480);
	colourImage.draw(10, 500, 640, 480);

	int xoffs=660;
	for (int i=0;i<depthTiles.size();i++){
		depthTiles[i].draw(xoffs,10,depthTiles[i].getWidth(),depthTiles[i].getHeight());
		colourTiles[i].draw(xoffs,500,colourTiles[i].getWidth(),colourTiles[i].getHeight());
		xoffs+=(colourTiles[i].getWidth()+10);
	}

	*/


	/*
	
	// draw from the live kinect
	kinect.drawDepth(10, 10, 400, 300);
	kinect.draw(420, 10, 400, 300);
	
	depthImage.draw(10, 320, 400, 300);
		
	
	// draw instructions
	ofSetColor(255, 255, 255);
	stringstream reportStream;
    
	reportStream << "press p to switch between images and point cloud, rotate the point cloud with the mouse" << endl
	<< "set far threshold " << farThreshold 
	<< ", fps: " << ofGetFrameRate() << endl
	<< "press c to close the connection and o to open it again, connection is: " << kinect.isConnected() << endl;

    if(kinect.hasCamTiltControl()) {
    	reportStream << "press UP and DOWN to change the tilt angle: " << angle << " degrees" << endl
        << "press 1-5 & 0 to change the led mode" << endl;
    }
    
	ofDrawBitmapString(reportStream.str(), 20, 652);

	*/
    
}



//--------------------------------------------------------------
void ofApp::exit() {
	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;
	}
}

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