//This program intends to get all the point depths from a frame then dump them to std out and close.
#include <fcntl.h>
#include <libfreenect.h>
#include <libfreenect_sync.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include "svg.h"
//gcc -Wall -g -o "%e" "%f"  -lfreenect -lfreenect_sync   
 int dtime=5;
 int arduino;
 
//int current_tilt=&current_tilt);
freenect_context *f_ctx;
freenect_device *f_dev;
	

    
#define WIDTH 640
#define HEIGHT 400

     int die = 0;

int kInit();                 //initiallize / test kinect library and device connection
void kClose();         //close the kinect device
void depth_cb(freenect_device *dev, void *depth, uint32_t timestamp);
void no_kinect_quit(void);            

int main(){

	kInit();  //init kinect lib, and device

    // Set the depth callback function
    freenect_set_depth_callback(f_dev, depth_cb);

    // Start the depth stream
    freenect_start_depth(f_dev);
    
     while (!die)    {
        // Update the Kinect device
        if (freenect_process_events(f_ctx) < 0){
            break;
		}
	    }

kClose();

    return 0;
}


//Kinect Stuff
int kInit(){
	    // Initialize libfreenect
    if (freenect_init(&f_ctx, NULL) < 0)    {
        printf("Failed to initialize libfreenect!\n");
        exit(1);
    }

    // Set the log level (optional)
  //  freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);
  
    // Open the Kinect device
    if (freenect_open_device(f_ctx, &f_dev, 0) < 0)    {
        printf("Failed to open Kinect device!\n");
       exit(1);
    }
    return 0;
	}
	
void kClose(){
		//shutdown functions
    freenect_stop_depth(f_dev);
    freenect_close_device(f_dev);
    freenect_shutdown(f_ctx);
		}
		
void depth_cb(freenect_device *dev, void *depth, uint32_t timestamp){
   svg* psvg;
    psvg = svg_create(WIDTH, HEIGHT);
    // Cast the depth data to unsigned short (16-bit) xbox uses 11 bits of depth
    uint16_t *depth_data = (uint16_t *)depth;
   // uint16_t depth_value = depth_data[index];
    uint16_t depth_value = 0;
if(depth_value>=2046){
	//nearing buffer overflow. this happens as object approaches camera
	depth_value=2046;
	}
    // Print the depth value at a specific pixel (e.g., pixel at (320, 240) (Center of frame))
    int x = 0;
    int y = 0;
    int index = 0; //index 0 to 307840 

for (x=0; x<=WIDTH; x+=4){  //I expect 640 numbers by 480
	index = y*WIDTH+x;  
	depth_value = depth_data[index];  
	//printf("%d ",depth_value);
  //printf(" %d %d ", x, y);
    svg_pixelDepth(psvg, x, y, depth_value);
	if(x>=WIDTH-1){
		if(y>=HEIGHT-1){
		svg_finalize(psvg);
        svg_save(psvg, "haaaah.svg");
        svg_free(psvg);
	    exit(1);
		}
		y+=4;  //goto next line
		x=0;
		printf("%d\n", y);
		}
	}
}
void no_kinect_quit(void){
	printf("Error: Kinect not connected?\n");
	exit(1);
}