/*

Markov Dynamic Prediction Code

Eduardo Navarro
Ben Peuker
Michael Quan

Supervised by Chris Clark
*/

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <math.h>

using namespace std;

//Determine Max Values for (x,y) axis in feet
#define MAX_X 200
#define MAX_Y 200

//Routers Used
#define NUM_ROUTERS 6
#define STATES 40000

//How many feet you move every iteration
#define STEP_VALUE 10

//Starting values for fingerprint on grid
#define STARTX 5
#define STARTY 5

//Small value close to 0 to set numbers to
#define SMALL_PROB 1.0E-10

//Number of steps to use for search
#define RADIUS 20

//PI isn't in the math lib, so here it is. 
const double PI = 4.0*atan(1.0);

//Probability grid for first run
static double prob[MAX_X][MAX_Y];

//After first run, the grid is split with the top 3 coordinates into new grids
static double probd1[MAX_X][MAX_Y];
static double probd2[MAX_X][MAX_Y];
static double probd3[MAX_X][MAX_Y];

//boolean grid to see if (x,y) coordinate is on the fingerprint
static bool fp_mask[MAX_X][MAX_Y];

//3 separate (x,y) predictions that are updated each iteration
int pred_x1 = 0;
int pred_y1 = 0;
int pred_x2 = 0;
int pred_y2 = 0;
int pred_x3 = 0;
int pred_y3 = 0;

//probability values related to the 3 separate predictions
double highest_prob = 0;
double second_prob = 0;
double third_prob = 0;

//row and col variables for loop iterations
int row = 0;
int col = 0;

//variable to check to see if it is the first iteration
bool first = true;


//structure containing average and standard deviation of fingerprint
struct rssi_fp
{
	double rssi_avg[NUM_ROUTERS];
	double rssi_stdev[NUM_ROUTERS];
};

//structure of current RSSI readings
struct rssi_tuple {
       double rssi[NUM_ROUTERS];
};

//structure to store (x,y) coordinate predictions
struct loc {
       int x;
       int y;
};

//vectors to push in locations
vector <loc> loc_set_1;
vector <loc> loc_set_2;
vector <loc> loc_set_3;

//the only location struct ever used in this program. just a temp to push variables into vector
loc newLoc;

//Get the normalization Gausiann of any input. This is used in the other functions
double get_prob_fp(int x, int y, rssi_tuple reading, rssi_fp rp[MAX_X][MAX_Y]) {
	double prob = 0;
	double total = 0;
	double gaussian = 0;
	double top = 0;
	double bottom = 0;
	double rooted = 0;

	for(int i = 0; i < NUM_ROUTERS; i++) {

		// (x - mu)^2 term
		top = (reading.rssi[i] - rp[x][y].rssi_avg[i]);
		top = top * top;

		//cout << "RSSI: " << (double)rssi[i] << endl;
		//cout << "RSSI AVG: " << rp[x][y].rssi_avg[i] << endl;
		//cout << "TOP: " << top << endl;

		// 2*sigma^2 term
		bottom = rp[x][y].rssi_stdev[i];
		bottom = 2 * bottom * bottom;

		//cout << "BOTTOM: " << bottom << endl;

		// sqrt(2*pi*sigma^2) term
		rooted = rp[x][y].rssi_stdev[i];
		rooted = 2 * PI * rooted * rooted;
		rooted = sqrt(rooted);

		//cout << "ROOTED: " << rooted << endl;

		// gaussian distribution function
		gaussian = (1/rooted) * exp(0-(top/bottom));

		//cout << "GAUSSIAN: " << gaussian << endl;

		total += gaussian;
	}

	return total;
}

//The predict static function was never used and is incomplete at this point. It is only here for future development
void pred_static(rssi_tuple reading, rssi_fp rp[MAX_X][MAX_Y])
{
	double prob_static = 0;
	char pred_x = 0;
	char pred_y = 0;
	double highest_probs = 0;
	
	double row_states_static = 0;
	double col_states_static = 0;
	double total_states_static = 0;
	//count number of states
	for (row = STARTX; row <= MAX_X; row += STEP_VALUE)
	{
        row_states_static ++;
		for (col = STARTY; col <= MAX_Y; col += STEP_VALUE)
		{
			col_states_static ++;
		}
	}
	
	total_states_static = row_states_static * col_states_static;

	// get probability of being at any x,y location given the rssi readings
	for (row = STARTX; row <= MAX_X; row += STEP_VALUE)
	{
		for (col = STARTY; col <= MAX_Y; col += STEP_VALUE)
		{
            if(fp_mask[row][col]) {
                 prob[row][col] = get_prob_fp(row,col,reading,rp) * 1/total_states_static;
            //     cout << fp_mask[row][col] << endl;
            } else {
                   prob[row][col] = SMALL_PROB;
               //    cout << "else" << endl;
            }
            
		}
	}

	for (row = STARTX; row <= MAX_X; row += STEP_VALUE)
	{
		for (col = STARTY; col <= MAX_Y; col += STEP_VALUE)
		{
			if (prob[row][col] > highest_probs)
			{
				pred_x = row;
				pred_y = col;
				highest_probs = prob[row][col];
			}
		}
	}

}

void pred_dynamic(rssi_tuple reading, rssi_fp rp[MAX_X][MAX_Y]) {

	//FIRST RUN: Populate grid evenly, run prediction step on it. 
	if(first == true) {
             
        //count number of states
        double row_states_dynamic = 0;
	    double col_states_dynamic = 0;
	    double total_states_dynamic = 0;     
	    
	    for (row = STARTX; row <= MAX_X; row += STEP_VALUE)
        {
            row_states_dynamic++;
		    for (col = STARTY; col <= MAX_Y; col += STEP_VALUE)
		    {
			    col_states_dynamic++;
             }
	    }
		//total states = rows * cols
		total_states_dynamic = row_states_dynamic * col_states_dynamic;
             
		//STEP 1 for all X,Y prob[x][y] = 1/States
		for (row = STARTX; row <= MAX_X; row += STEP_VALUE) {
			for (col = STARTY; col <= MAX_Y; col += STEP_VALUE) {
			
			if(fp_mask[row][col]) {
                 prob[row][col] = 1/total_states_dynamic;
                 //cout << fp_mask[row][col] << endl;
                 
            } else {
                   prob[row][col] = SMALL_PROB;
                   //cout << "else " << prob[row][col] << endl;
            }

			}

		}
		
		//get first probability given even distribution
		for (row = STARTX; row <= MAX_X; row += STEP_VALUE) {
		    for (col = STARTY; col <= MAX_Y; col += STEP_VALUE) {
			    prob[row][col] *= get_prob_fp(row,col,reading,rp);
			    
			    //If probability is on the fingerprint, do correction or else just set very small
    		    if(fp_mask[row][col]) {
                     prob[row][col] = get_prob_fp(row,col,reading,rp) * 1/total_states_dynamic;
                
                } else {
                       prob[row][col] = SMALL_PROB;
                       //cout << "else " << prob[row][col] << endl;
                }
    
    			//cout << "prob after correction " << row << "," << col << " " << fp_mask[row][col] << " " << prob[row][col] << endl;
			    
		     }
		    //system("PAUSE");
        }
        
       	double sum = 0;

	     //Normalize before getting data points
		//add up sum of prob
		for(row = STARTX; row <= MAX_X; row += STEP_VALUE){
			for(col = STARTY; col <= MAX_Y; col += STEP_VALUE) {
				sum = sum + prob[row][col];
			}
		}
		//normalize cells
		for(row = STARTX; row <= MAX_X; row += STEP_VALUE){
			for(col = STARTY; col <= MAX_Y; col += STEP_VALUE) {
				prob[row][col] = (prob[row][col])/sum;
			}
		//	system("PAUSE");
		}
        
        //get top 3 data points from corrected grid
		for(row = STARTX; row <= MAX_X; row += STEP_VALUE) {
			for(col = STARTY; col <= MAX_Y; col += STEP_VALUE) {
				if(prob[row][col] > highest_prob) {
					//Update probabilities
					pred_x3 = pred_x2;
					pred_y3 = pred_y2;
					third_prob = second_prob;
					pred_x2 = pred_x1;
					pred_y2 = pred_y1;
					second_prob = highest_prob;
					pred_x1 = row;
					pred_y1 = col;
					highest_prob = prob[row][col];
					
		        }
				
				if( (prob[row][col] > second_prob) && (prob[row][col] < highest_prob)) {
					//Update probabilities
					pred_x3 = pred_x2;
					pred_y3 = pred_y2;
					third_prob = second_prob;
					pred_x2 = row;
					pred_y2 = col;
					second_prob = prob[row][col];
					
				}
				
				if( (prob[row][col] > third_prob) && (prob[row][col] < second_prob)) {
					//Update probabilities
					pred_x3 = row;
					pred_y3 = col;
					third_prob = prob[row][col];
					
				}
				
			}
		}
		
		//cout << "highest start " << pred_x1 << "," << pred_y1 << endl;
		//cout << "second start" << pred_x2 << "," << pred_y2 << endl;
		//cout << "lowest start" << pred_x3 << "," << pred_y3 << endl;
		
		//Take top (x,y) coordinate and put in in the vector.	
       	newLoc.x = pred_x1;
	   	newLoc.y = pred_y1;
	   	loc_set_1.push_back(newLoc);
	   
	   	//same thing
       	newLoc.x = pred_x2;
	   	newLoc.y = pred_y2;
	   	loc_set_2.push_back(newLoc);
	   
	   	//same
       	newLoc.x = pred_x3;
	   	newLoc.y = pred_y3;
	   	loc_set_3.push_back(newLoc);
		
		
		first = false;	
		
		
		//The second and all iterations happen in the code below...
	} 	else {
			highest_prob = 0;
			second_prob = 0;
			third_prob = 0;
		
			//form low probs for entire grid 
			for(row = STARTX; row <= MAX_X; row += STEP_VALUE) {
				for(col = STARTY; col <= MAX_Y; col += STEP_VALUE) {
					 probd1[row][col] = SMALL_PROB;
				 }
			}
		
			//form low probs for entire d2 grid
			for(row = STARTX; row <= MAX_X; row += STEP_VALUE) {
				for(col = STARTY; col <= MAX_Y; col += STEP_VALUE) {
					 probd2[row][col] = SMALL_PROB;
				 }
			}
			
			//form low probs for entire d3 grid
			for(row = STARTX; row <= MAX_X; row += STEP_VALUE) {
				for(col = STARTY; col <= MAX_Y; col += STEP_VALUE) {
					 probd3[row][col] = SMALL_PROB;
				 }
			}
		
			//flatten radius grid
			//SET EVERYTHING EVEN IN REIGON. FLATTEN
			for (row = pred_x1-RADIUS; row <= pred_x1+RADIUS; row += STEP_VALUE) {
	            for (col = pred_y1 - RADIUS; col <= pred_y1 + RADIUS; col += STEP_VALUE) {
	    			if(row < 0 || col < 0 || row >200 || col > 200) {
	                } else {
	                probd1[row][col] = .1;
	                }
	    		}
	    	}
    	
	    	for (row = pred_x2-RADIUS; row <= pred_x2+RADIUS; row += STEP_VALUE) {
	    		for (col = pred_y2-RADIUS; col <= pred_y2+RADIUS; col += STEP_VALUE) {
	    			if(row < 0 || col < 0 || row >200 || col > 200) {
	                } else {
	                probd2[row][col] = .1;
	                }
	    		}
	    	}
	    	
	    	for (row = pred_x3-RADIUS; row <= pred_x3+RADIUS; row += STEP_VALUE) {
	    		for (col = pred_y3-RADIUS; col <= pred_y3+RADIUS; col += STEP_VALUE) {
	    			if(row < 0 || col < 0 || row >200 || col > 200) {
	                } else {
	                probd3[row][col] = .1;
	                }
	    		}
    		}
	
			//STEP 5 do correction step for all 3
			//for first time, correct entire grid. for all other times, only correct radius
			//get probability and update probability grid. do not correct values not on fingerprint
			for (row = pred_x1-RADIUS; row <= pred_x1+RADIUS; row += STEP_VALUE) {
				for (col = pred_y1 - RADIUS; col <= pred_y1 + RADIUS; col += STEP_VALUE) {
				    
		            if(row < 0 || col < 0 || row >200 || col > 200) {
		            } else {
                        if(fp_mask[row][col]) {
		                 	probd1[row][col] *= get_prob_fp(row,col,reading,rp);
		               //  cout << fp_mask[row][col] << endl;
		            		} else {
		                   probd1[row][col] = SMALL_PROB;
		            		}
		            } //end low value check
					
					
				}
			}
		
			for (row = pred_x2-RADIUS; row <= pred_x2+RADIUS; row += STEP_VALUE) {
				for (col = pred_y2-RADIUS; col <= pred_y2+RADIUS; col += STEP_VALUE) {
					
		            if(row < 0 || col < 0 || row >200 || col > 200) {
		            } else {
		                
		            	if(fp_mask[row][col]) {
		                	probd2[row][col] *= get_prob_fp(row,col,reading,rp);
		               	//  cout << fp_mask[row][col] << endl;
		            	} else {
		                   probd2[row][col] = SMALL_PROB;
		            	}
		            }//end low value check
				}
			}
			
			for (row = pred_x3-RADIUS; row <= pred_x3+RADIUS; row += STEP_VALUE) {
				for (col = pred_y3-RADIUS; col <= pred_y3+RADIUS; col += STEP_VALUE) {
					
		            if(row < 0 || col < 0 || row >200 || col > 200) {
		            } else {
		                
		            	if(fp_mask[row][col]) {
		                 	probd3[row][col] *= get_prob_fp(row,col,reading,rp);
		            	} else {
		                   probd3[row][col] = SMALL_PROB;
		            	}
		            } //end low value check
				}
			}

			//NORMALIZE ALL AGAIN
			double sum1cur = 0;
	
			//Normalize 1
			//add up sum of prob
			for(row = STARTX; row <= MAX_X; row += STEP_VALUE){
				for(col = STARTY; col <= MAX_Y; col += STEP_VALUE) {
					sum1cur = sum1cur + probd1[row][col];
				}
			}
			//normalize cells
			for(row = STARTX; row <= MAX_X; row += STEP_VALUE){
				for(col = STARTY; col <= MAX_Y; col += STEP_VALUE) {
					probd1[row][col] = (probd1[row][col])/sum1cur;
					
					//cout << "normalized values: " << probd3[row][col] << endl;
					
				}
				//system("PAUSE");
			}
	
			double sum2cur = 0;
			
			//Normalize 2
			//add up sum of prob
			for(row = STARTX; row <= MAX_X; row += STEP_VALUE){
				for(col = STARTY; col <= MAX_Y; col += STEP_VALUE) {
					sum2cur = sum2cur + probd2[row][col];
				}
			}
			//normalize cells
			for(row = STARTX; row <= MAX_X; row += STEP_VALUE){
				for(col = STARTY; col <= MAX_Y; col += STEP_VALUE) {
					probd2[row][col] = (probd2[row][col])/sum2cur;
				}
			}
	
			double sum3cur = 0;
			
            //Normalize 3
			//add up sum of prob
			for(row = STARTX; row <= MAX_X; row += STEP_VALUE){
				for(col = STARTY; col <= MAX_Y; col += STEP_VALUE) {
					sum3cur = sum3cur + probd3[row][col];
				}
			}
			//normalize cells
			for(row = STARTX; row <= MAX_X; row += STEP_VALUE){
				for(col = STARTY; col <= MAX_Y; col += STEP_VALUE) {
					probd3[row][col] = (probd3[row][col])/sum3cur;
					
					
				}
			}
			
			//Predict Values for next round
		    for (row = pred_x1-RADIUS; row <= pred_x1+RADIUS; row += STEP_VALUE) {
			    for (col = pred_y1-RADIUS; col <= pred_y1+RADIUS; col += STEP_VALUE) {
				 
	             	if(row < 0 || col < 0 || row >200 || col > 200) {
	                    
	             	} else {
	                	if (probd1[row][col] > highest_prob)
				        {
	        		 	    pred_x1 = row;
	        		  	    pred_y1 = col;
	        		   	    highest_prob = probd1[row][col];
			            }
	                }
				}
		   	}
			
		   	newLoc.x = pred_x1;
		  	newLoc.y = pred_y1;
		  	loc_set_1.push_back(newLoc);
	   
		    for (row = pred_x2-RADIUS; row <= pred_x2+RADIUS; row += STEP_VALUE) {
			    for (col = pred_y2-RADIUS; col <= pred_y2+RADIUS; col += STEP_VALUE) 
			    {
				 
	             	if(row < 0 || col < 0 || row >200 || col > 200) {
	             
	              	} else {
	                  
		            	if (probd2[row][col] > second_prob) {
					 		pred_x2 = row;
					  		pred_y2 = col;
					   		second_prob = probd2[row][col];
				    	}
					}
	        	}
		   	}
	   
		   newLoc.x = pred_x2;
		   newLoc.y = pred_y2;
		   loc_set_2.push_back(newLoc);
	   
		   for (row = pred_x3-RADIUS; row <= pred_x3+RADIUS; row += STEP_VALUE) {
			    for (col = pred_y3-RADIUS; col <= pred_y3+RADIUS; col += STEP_VALUE) 
			    {
				 
	             if(row < 0 || col < 0 || row >200 || col > 200) {
	             
	           	} else {
		           	if (probd3[row][col] > highest_prob)
				 	{
					 	pred_x3 = row;
					  	pred_y3 = col;
					   	third_prob = probd3[row][col];
			    	}
				}
			}
		   }
	
	       newLoc.x = pred_x3;
		   newLoc.y = pred_y3;
		   loc_set_3.push_back(newLoc);
				
	} //end else		
	

} //end dynamic

int main(int argc[], char* argv[]) {

    cout << "staring to run" << endl;
	ifstream fin;
	ifstream fin2;
	ifstream fin3;
	string line;
	//input files
	string infilename1 = "avg_10f_tr.csv";
	string infilename2 = "stdev_10f_tr.csv";
	string infilename3 = "markov_input3.csv";
	fin.open(infilename1.c_str());
	fin2.open(infilename2.c_str());
	fin3.open(infilename3.c_str());
	
    //output files in csv format
    ofstream fout_dyn3;
    fout_dyn3.open("markov_fake3_third_20r.csv");
    ofstream fout_dyn2;
    fout_dyn2.open("markov_fake3_second_20r.csv");
    ofstream fout_dyn1;
    fout_dyn1.open("markov_fake3_highest_20r.csv");
	
    string token;
	
	static rssi_fp fp[MAX_X][MAX_Y];
	
	//initialize bool [][] to 0s
    for (int row = 0; row < MAX_X; row++ ) {
         for (int col = 0; col < MAX_Y; col++) {
             fp_mask[row][col] = false;
         }
     }             
     
     for (int row = 0; row < MAX_X; row++ ) {
         for (int col = 0; col < MAX_Y; col++) {
             for(int router = 0; router < NUM_ROUTERS; router++) {
                     fp[row][col].rssi_avg[router] = 0;
                     fp[row][col].rssi_stdev[router] = 0;
             }
         }
     }
                     
	
	// initialize above array to zeroes
	
	int curr_x = 0;
	int curr_y = 0;
	double rssi_reading;
	
	
	//get average rssi fingerprint from file
	while (fin)
	{
        
		// get x coord
		getline(fin, token, ',');
		curr_x = atoi(token.c_str());
		
		// get y coord
		getline(fin, token, ',');
		curr_y = atoi(token.c_str());
		


        // get rssi 1 to rssi 6
        getline(fin, token, ',');
        rssi_reading = atof(token.c_str());
        fp[curr_x][curr_y].rssi_avg[0] = rssi_reading;
        
        getline(fin, token, ',');
        rssi_reading = atof(token.c_str());
        fp[curr_x][curr_y].rssi_avg[1] = rssi_reading;
        
        getline(fin, token, ',');
        rssi_reading = atof(token.c_str());
        fp[curr_x][curr_y].rssi_avg[2] = rssi_reading;
        
        getline(fin, token, ',');
        rssi_reading = atof(token.c_str());
        fp[curr_x][curr_y].rssi_avg[3] = rssi_reading;
        
        getline(fin, token, ',');
        rssi_reading = atof(token.c_str());
        fp[curr_x][curr_y].rssi_avg[4] = rssi_reading;
        
        getline(fin, token, '\n');
        rssi_reading = atof(token.c_str());
        fp[curr_x][curr_y].rssi_avg[5] = rssi_reading;
        
        //set mask to high for current (x,y) coordinate
		
		if (fp[curr_x][curr_y].rssi_avg[0] != 0 ||
			fp[curr_x][curr_y].rssi_avg[1] != 0 ||
			fp[curr_x][curr_y].rssi_avg[2] != 0 ||
			fp[curr_x][curr_y].rssi_avg[3] != 0 ||
			fp[curr_x][curr_y].rssi_avg[4] != 0 ||
			fp[curr_x][curr_y].rssi_avg[5] != 0)
		{
			fp_mask[curr_x][curr_y] = true;
		}
		
		
        
	}

	//get std dev rssi from fingerprint
	while (fin2)
	{
	    // get x coord
		getline(fin2, token, ',');
		curr_x = atof(token.c_str());
		
		// get y coord
		getline(fin2, token, ',');
		curr_y = atof(token.c_str());

        // get rssi 1 to rssi 6
        getline(fin2, token, ',');
        rssi_reading = atof(token.c_str());
        fp[curr_x][curr_y].rssi_stdev[0] = rssi_reading;
        
        getline(fin2, token, ',');
        rssi_reading = atof(token.c_str());
        fp[curr_x][curr_y].rssi_stdev[1] = rssi_reading;
        
        getline(fin2, token, ',');
        rssi_reading = atof(token.c_str());
        fp[curr_x][curr_y].rssi_stdev[2] = rssi_reading;
        
        getline(fin2, token, ',');
        rssi_reading = atof(token.c_str());
        fp[curr_x][curr_y].rssi_stdev[3] = rssi_reading;
        
        getline(fin2, token, ',');
        rssi_reading = atof(token.c_str());
        fp[curr_x][curr_y].rssi_stdev[4] = rssi_reading;
        
        
        getline(fin2, token, '\n');
        rssi_reading = atof(token.c_str());
        fp[curr_x][curr_y].rssi_stdev[5] = rssi_reading;

    }
    
    //read in sample data to run prediction
    
    rssi_tuple temp;
    vector <rssi_tuple> rssi_vector;
    
    //populate with inputs
    while(fin3)
    {
        getline(fin3, token, ',');
        rssi_reading = (char)atof(token.c_str());
        temp.rssi[0]  = rssi_reading; 
        getline(fin3, token, ',');
        rssi_reading = (char)atof(token.c_str());
        temp.rssi[1]  = rssi_reading;
        getline(fin3, token, ',');
        rssi_reading = (char)atof(token.c_str());
        temp.rssi[2]  = rssi_reading;
        getline(fin3, token, ',');
        rssi_reading = (char)atof(token.c_str());
        temp.rssi[3]  = rssi_reading;
        getline(fin3, token, ',');
        rssi_reading = (char)atof(token.c_str());
        temp.rssi[4]  = rssi_reading;
        getline(fin3, token, '\n');
        rssi_reading = (char)atof(token.c_str());
        temp.rssi[5]  = rssi_reading;
        
        rssi_vector.push_back(temp);

    }
    
    //actually does the prediction for all values
    for(int j = 0; j < rssi_vector.size();j++) {
            pred_dynamic(rssi_vector[j] ,fp); 
    }
    
	   //add predictions to vectors
	for(int k = 0; k < rssi_vector.size(); k++) {
	           fout_dyn1 << loc_set_1[k].x << "," << loc_set_1[k].y << endl;
	           fout_dyn2 << loc_set_2[k].x << "," << loc_set_2[k].y << endl; 
	           fout_dyn3 << loc_set_3[k].x << "," << loc_set_3[k].y << endl;

    }      
    
	//close files
    fout_dyn2.close();
    fout_dyn1.close();
    fout_dyn3.close();
    
	system("PAUSE");

	return 0;
}