/*
* This program sends characters to the Arduino which is listening to the serial port for 
* Character input to control motor functions. Depending on depth detected, start or stop the 
* motors. For use in Robot navigation using the Xbox360 kinect
*/
#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>
//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 480
//(0,0) is top left also index 0
     char w='w';
     char a='a';
     char s='s';
     char d='d';
     char b='b';
int die = 0;
int kInit();
void kClose();
void go(char x);
void depth_cb(freenect_device *dev, void *depth, uint32_t timestamp);
void no_kinect_quit(void);
int main(){
	kInit();  //init kinect lib, and device
    arduino = open("/dev/ttyACM0", O_RDWR | O_NOCTTY | O_NDELAY);
    if (arduino == -1) {
        perror("Is the Arduino Connected?");
        return 1;
    }
    struct termios options;
    tcgetattr(arduino, &options);
    cfsetispeed(&options, B9600);
    cfsetospeed(&options, B9600);
    options.c_cflag |= (CLOCAL | CREAD);
    tcsetattr(arduino, TCSANOW, &options);

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

void go(char x){
	 char keystrokes[] = "b"; 
	  keystrokes[0]=x;
  //  printf("%c\n",x);
    write(arduino, keystrokes, strlen(keystrokes));
  //  sleep(dtime);
	}

//Kinect Stuff
int kInit(){
	    // Initialize libfreenect
    if (freenect_init(&f_ctx, NULL) < 0)    {
        printf("Failed to initialize libfreenect!\n");
        return 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");
        return 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){
    // Cast the depth data to unsigned short (16-bit)
    uint16_t *depth_data = (uint16_t *)depth;

    // Print the depth value at a specific pixel (e.g., pixel at (320, 240) (Center of frame))
    int x = 320;
    int y = 240;
    int index = y * WIDTH + x;  //index 0 to 307840 
    uint16_t depth_value = depth_data[index];
if(depth_value>=2046){
	//nearing buffer overflow. this happens as object approaches camera
	depth_value=2046;
	}

if(depth_value>600&& depth_value<2000){
	printf("%u\n", depth_value);
	go(w);
	}
	if(depth_value>2000){
	printf("%u\n", depth_value);
	go(b);
	}

    // You can further process the depth data or perform other actions here
}
void no_kinect_quit(void){
	printf("Error: Kinect not connected?\n");
	exit(1);
}