//ESand somewhere between working and having features
#include <SDL/SDL.h>
#include <libfreenect.h>
#include <libfreenect_sync.h>
#include <stdio.h>
#include <unistd.h>  //tcgetattr and sleep
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>

//g++ -g -Wall -o "ppplotKinect" "ppplotKinect.cpp"  -lSDL -lSDL_ttf -lSDL_mixer -lSDL_image   -lfreenect -lfreenect_sync 
SDL_Surface* Backbuffer = NULL;
 /*
  * Config file: all integers for now, file: pplot.conf
  * initX,Y: relate to bounding area of the sensor. if we dont need to read more data points, dont.
  * pixelation reffers to limiting resolution by skipping pixels, which results in pixelation. 
  * pixelation, brightness, minDepth, maxDepth, depthScalling. 
  * pxX,pxY,brightness,minD, maxD, DScale, initX1,initX2, initY1, initY2
  * */
freenect_context *f_ctx;
freenect_device *f_dev;
#define WIDTH 640
#define HEIGHT 480

    //sysConfig vars
    int pxX=0;
    int pxY=0;
    int brightness=30;
    int minD=0;
    int maxD=2047;
    int DScale=1;
    int initX1=0;
    int initY1=0;
    int initX2=640;
    int initY2=480;
    int accelThreshX=0;
    int accelThreshY=0;
    int accelThreshZ=0;
    

using namespace std;
     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);     
bool ProgramIsRunning();
void DrawPixel(SDL_Surface *surface, int x, int y , unsigned int depth_value);
void IR(freenect_device *dev, uint16_t brightness);
int getCFG(string configFile);

int main(int argc, char* args[]){
	//read config file, and assign variables, unless the config file isnt pressent. 
	    string configFile="pplot.cfg";
getCFG(configFile);     //get or reset config variables from file or use system

    if(SDL_Init(SDL_INIT_EVERYTHING) < 0){
        printf("Failed to initialize SDL!\n");
        return 0;
    }
    kInit();  //init kinect lib, and device
    	int ire = freenect_get_ir_brightness(f_dev);
		printf("%d brightness\n", ire);
    //freenect_start_ir(f_dev);
    IR(f_dev,30);  //brightness 1 to 50 may effect resolution depending on conditions.

    freenect_set_depth_callback(f_dev, depth_cb);
    // Start the depth stream
    freenect_start_depth(f_dev);
    Backbuffer = SDL_SetVideoMode(640 ,480, 32, SDL_SWSURFACE);
    SDL_WM_SetCaption("Ghost Data", NULL);
    while(ProgramIsRunning())    {
		        if (freenect_process_events(f_ctx) < 0){
            break;
		}
    }
   SDL_Quit();
    kClose();
    return 0;
}

void depth_cb(freenect_device *dev, void *depth, uint32_t timestamp){
    // Cast the depth data to unsigned short (16-bit) xbox uses 11 bits of depth
    uint16_t *depth_data = (uint16_t *)depth;

unsigned int depth_value =0;
    int x = 0;
    int y = 0;
    int index = 0; //index 0 to 307840 

for (x=0; x<=WIDTH; x+=pxX){   //pxX cant B 3 I guess
     index = y*WIDTH+x;  
	depth_value = depth_data[index];  
    DrawPixel(Backbuffer, x,y, depth_value);

	if(x>=WIDTH){
		if(y>=HEIGHT){
	    x=0;
	    //y+=pxY;
	    y=0;
	    SDL_Flip(Backbuffer);	
	    return;
		}
		y+=pxY;  //goto next line: pxY
		x=0;
		}     
	}
}
bool ProgramIsRunning(){
    SDL_Event event;
	int brightness = freenect_get_ir_brightness(f_dev);

    bool running = true;
    while(SDL_PollEvent(&event))   {
        if(event.type == SDL_QUIT)
            running = false;
    }
    if(event.type==SDL_KEYDOWN){
		switch(event.key.keysym.sym){
			case SDLK_ESCAPE :
					running=false;
					break;
					
			case SDLK_a : 
			//get brightness add 1
			brightness = freenect_get_ir_brightness(f_dev);
		    freenect_set_ir_brightness(f_dev, brightness+1);
		    printf("brightness: %d \n", brightness);
		    if (brightness >=50){
				brightness=50;
				break;
				}
			break;
			case SDLK_d :
			//get brightness sub 1
			brightness = freenect_get_ir_brightness(f_dev);
		    freenect_set_ir_brightness(f_dev, brightness-1);
		    printf("brightness: %d \n", brightness);
		    if (brightness <=1){
				brightness=1;
				break;
				}
			break; 
				
             default : 
                    break;			
			}
		}

    return running;
}

void DrawPixel(SDL_Surface *surface,int x , int y , unsigned  int depth_value){
    if(SDL_MUSTLOCK(surface)){
        if(SDL_LockSurface(surface) < 0)
            return;
    }
    Uint32 *buffer;
    Uint32 color;

    color = SDL_MapRGB(surface->format, 0, 0, 0);
    if(x >= surface->w || x < 0 || y >= surface->h || y < 0)
        return;
//write a switch to convert depth to rgb values.
switch (depth_value) {
    //depth value fckery
    case 0 ... 400:
    color = SDL_MapRGB(surface->format, depth_value+100,0,0);  //red
    break;
    
    case 401 ... 624:
  color = SDL_MapRGB(surface->format, 255,depth_value/3,0); //orange scale
    break;
    
    case 625 ... 824:
    color = SDL_MapRGB(surface->format, depth_value-300,depth_value-300,0);  //yello
    break;
    
    case  825 ... 925:
    color = SDL_MapRGB(surface->format, 0,depth_value/5,0); //green scale
    break;
      
    case 926 ... 1480:
    color = SDL_MapRGB(surface->format, depth_value/20,0,depth_value/7); //indiogo
    break;
    
    case 1481 ... 1999:
    color = SDL_MapRGB(surface->format, 0,255, 255);
    break;

   case 2000 ... 2047:
        color = SDL_MapRGB(surface->format, depth_value,depth_value,depth_value); 
     break;
    default:
color = SDL_MapRGB(surface->format, 0,0,0);
    break;
        }

    buffer = (Uint32*)surface->pixels  + y*surface->pitch/4+x;
    (*buffer) = color;

    if(SDL_MUSTLOCK(surface))
        SDL_UnlockSurface(surface);
}
//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 IR(freenect_device *dev, uint16_t brightness){
		int ire = freenect_get_ir_brightness(f_dev);
		freenect_set_ir_brightness(f_dev, brightness);
		printf("%d brightness\n", ire);
		cout << "pxX: " << pxX << " pxY:"<<pxY <<endl;
		//int freenect_set_ir_brightness(f_dev,  brightness);
		}

void kClose(){
		//shutdown functions
    freenect_stop_depth(f_dev);
    freenect_close_device(f_dev);
    freenect_shutdown(f_ctx);
   // SDL_Quit();
		}
void no_kinect_quit(void){
	printf("Error: Kinect not connected?\n");
	exit(1);
}

//Get ConFiG File
int getCFG(string x){
	    ifstream inputFile(x); // Replace "data.csv" with your CSV file's name
    if (!inputFile.is_open()) {
        std::cerr << "Failed to open the file, Using system values." << std::endl;
    }
 /*
  * Config file: all integers for now, file: pplot.conf
  * initX,Y: relate to bounding area of the sensor. if we dont need to read more data points, dont.
  * pixelation reffers to limiting resolution by skipping pixels, which results in pixelation. 
  * pixelation, brightness, minDepth, maxDepth, depthScalling. xyLimits, accelorometerThreshohlds. 
  * 
  * pxX,pxY,brightness,minD, maxD, DScale, initX1,initX2, initY1, initY2,accellThreshX,Y,Z
  * 
  * */
    std::string line;
    if (std::getline(inputFile, line)) {
        std::istringstream ss(line);
        std::string xStr, yStr, brightnessStr,minDStr,maxDStr,DScaleStr,initX1Str,initX2Str,initY1Str,initY2Str, accelThreshXStr, accelThreshYStr, accelThreshZStr;
        //dont forget its all strings in the file
        if (getline(ss, xStr, ',') &&
            getline(ss, yStr, ',') &&
            getline(ss, brightnessStr, ',') &&
            getline(ss, minDStr, ',') &&
            getline(ss, maxDStr, ',') &&
            getline(ss, DScaleStr, ',') &&
            getline(ss, initX1Str, ',') &&
            getline(ss, initX2Str, ',') &&
            getline(ss, initY1Str, ',') &&
            getline(ss, initY2Str, ',')  && 
            getline(ss, accelThreshXStr , ',') &&
            getline(ss,  accelThreshYStr, ',') &&
            getline(ss, accelThreshZStr )) {
             pxX = stoi(xStr);
             pxY = stoi(yStr);
             brightness = stoi(brightnessStr);
             minD= stoi(minDStr);
             maxD=stoi(maxDStr);
             initX1=stoi(initX1Str);
             initX2=stoi(initX2Str);
             initY1=stoi(initY1Str);
             initY2=stoi(initY2Str);
             accelThreshX=stoi(accelThreshXStr);
             accelThreshY=stoi(accelThreshYStr);
             accelThreshZ=stoi(accelThreshZStr);
            //readd from file
           std::cout << "PxX: " << xStr << ", PxY: " << yStr << ", Brightness: " << brightness <<  " , minDepth: " << minD<< " maxDepth: "<< maxD << " Depth Scale:" << DScale 
           <<" \n initX1:" << initX1 <<" , initY1: "<< initY1 << " , initX2: " <<initX2 << " , initY2: "  << initY2 
           << "\nAccelorometer Threshholds X,Y,Z:"<< accelThreshX << ","<< accelThreshY << ","<<accelThreshZ << std::endl;
        } else {
            cerr << "Partialy read config file.\n filing in missing system Variables\n" ;
            cout << "PxX: " << pxX << ", PxY: " << pxY << ", Brightness: " << brightness <<  " , minDepth: " << minD<< " maxDepth: "<< maxD 
            <<" \n initX1:" << initX1 <<" , initY1: "<< initY1 << " , initX2: " <<initX2 << " , initY2: "  << initY2 
            << "\nAccelorometer Threshhold X, Y, Z: "<< accelThreshX << "," << accelThreshY << "," <<accelThreshZ << endl;
            }
    } else {
        cerr << "Empty file: using system Variables\n";
       cout << "PxX: " << pxX << ", PxY: " << pxY << ", Brightness: " << brightness <<  " , minDepth: " << minD<< "maxDepth: "<< maxD 
            <<" \n initX1:" << initX1 <<" , initY1: "<< initY1 << " , initX2: " <<initX2 << " , initY2: "  << initY2 
            << "\nAccelorometer Threshhold X, Y, Z: "<< accelThreshX << "," << accelThreshY << "," <<accelThreshZ << endl;   
             }
    inputFile.close();
    return 0;
	}