#include "ofApp.h"


//--------------------------------------------------------------
void ofApp::setup() {
	ofSetLogLevel(OF_LOG_VERBOSE);
	
	// 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);

		//how to draw a pyramid mosaic
	//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

	int min_w=ceil(kinect.width/(MAX_TILE_SIZE*2))*2;
	int min_h=ceil(kinect.height/(MAX_TILE_SIZE*2))*2;

	//get number of levels
	int levels=0;
	for (int i=MIN_TILE_SIZE;i>=MAX_TILE_SIZE;i*=2) {
		levels++;
		int w=ceil(kinect.width/(i*2))*2;
		int h=ceil(kinect.height/(i*2))*2;
		numTiles.push_back(make_pair(w,h));
		cerr<<"level "<<levels<<" mipmap: "<<w<<"x"<<h<<endl;
	}

	numTiles.resiz

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

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

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

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

		for (int i=MIN_TILE_SIZE,l=0;i>=MAX_TILE_SIZE;i*=2,l++){
			colourTiles[l].allocate(numTiles[l].first,numTiles[l].second);
			colourTiles[l].scaleIntoMe(*prevCol);
			depthTiles[l].allocate(numTiles[l].first,numTiles[l].second);
			depthTiles[l].scaleIntoMe(*prevDepth);
			//extend borders to a whole number of tiles of the next size
			if (l<colourTiles.size()-1){
				colourTiles[l].extend(numTiles[l+1].first*2,numTiles[l+1].second*2);
				depthTiles[l].extend(numTiles[l+1].first*2,numTiles[l+1].second*2);
			}
			prevCol=&colourTiles[l];
			prevDepth=&depthTiles[l];
		}
	}
}

//--------------------------------------------------------------
void ofApp::draw() {
	
	ofSetColor(255, 255, 255);
	
	// 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)
{}