#include <LiquidCrystal_I2C.h>
#include <Wire.h> 
#include <math.h> 

LiquidCrystal_I2C lcd(0x27, 20, 4);

#define BUTTON 2 
#define MAX_DATA 50 
#define K 9 

String classification_condition = "  "; 

float redValue = 248.0; 
float greenValue = 248.0; 
float blueValue = 248.0; 
float pHactual = 6.36; 
float distances[MAX_DATA];
int labels[MAX_DATA];
int counting_labels[MAX_DATA] = {0};
int max_counting = 0;  
int most_freq_label = -1;
int count = 1; 
int button_state = 0;  

unsigned long comptime; 

bool state = false;  

typedef struct Point {
  float r;
  float g;
	float b; 
	float ph;
  int label;  // Label kelas    
} Point;  

struct Point testingPoint; 
struct Point train_data[MAX_DATA] = {  
	{0.980, 0.965, 0.965, 0.969, 0}, {0.988, 0.941, 0.933, 0.961, 0}, {0.973, 0.957, 0.949, 0.971, 0}, {0.980, 0.957, 0.933, 0.974, 0},  
	{0.988, 0.976, 0.961, 0.960, 0}, {0.973, 0.965, 0.949, 0.953, 0}, {0.984, 0.965, 0.937, 0.964, 0}, {0.973, 0.957, 0.945, 0.960, 0},  
	{0.984, 0.957, 0.925, 0.946, 0}, {0.976, 0.937, 0.898, 0.950, 0}, {0.992, 0.945, 0.929, 0.956, 0}, {0.980, 0.941, 0.922, 0.949, 0},  
	{0.969, 0.949, 0.871, 0.934, 0}, {0.965, 0.937, 0.878, 0.933, 0}, {0.957, 0.933, 0.898, 0.939, 0}, {0.957, 0.945, 0.882, 0.936, 0},  
  {0.969, 0.925, 0.871, 0.920, 1}, {0.973, 0.937, 0.882, 0.917, 1}, {0.965, 0.933, 0.890, 0.914, 1}, {0.976, 0.949, 0.894, 0.911, 1},  
	{0.961, 0.933, 0.855, 0.904, 1}, {0.957, 0.918, 0.847, 0.897, 1}, {0.969, 0.937, 0.871, 0.896, 1}, {0.957, 0.914, 0.863, 0.901, 1},  
  {0.937, 0.898, 0.847, 0.874, 1}, {0.949, 0.922, 0.882, 0.870, 1}, {0.941, 0.902, 0.871, 0.883, 1}, {0.953, 0.918, 0.871, 0.879, 1},  
  {0.941, 0.898, 0.831, 0.956, 1}, {0.945, 0.902, 0.859, 0.851, 1}, {0.933, 0.898, 0.827, 0.860, 1}, {0.945, 0.914, 0.851, 0.853, 1},  
  {0.925, 0.871, 0.812, 0.836, 2}, {0.929, 0.918, 0.824, 0.836, 2}, {0.933, 0.894, 0.820, 0.841, 2}, {0.922, 0.914, 0.839, 0.841, 2},  
  {0.910, 0.859, 0.796, 0.814, 2}, {0.902, 0.863, 0.776, 0.807, 2}, {0.894, 0.871, 0.784, 0.814, 2}, {0.910, 0.871, 0.808, 0.821, 2},  
	{0.890, 0.831, 0.749, 0.791, 2}, {0.894, 0.855, 0.745, 0.787, 2}, {0.882, 0.839, 0.741, 0.783, 2}, {0.894, 0.855, 0.761, 0.796, 2},  
	{0.878, 0.855, 0.706, 0.754, 2}, {0.878, 0.831, 0.733, 0.759, 2}, {0.871, 0.824, 0.718, 0.770, 2}, {0.882, 0.835, 0.753, 0.766, 2},  
};  
// Point testingPoint;  

// int valueK = K; 
// int train_data_size = sizeof(train_data); 

int swapINTvalue(int *valuePrev, int *valueNext) {
	int temp;
	temp = *valuePrev; 
	*valuePrev = *valueNext; 
	*valueNext = temp; 
}

float swapFLTvalue(float *valuePrev, float *valueNext) {
	float temp; 
	temp = *valuePrev; 
	*valuePrev = *valueNext; 
	*valueNext = temp; 
} 

float euclideanDistances(struct Point train, struct Point tests) {
	float sqRed = pow((train.r - tests.r), 2); 
	float sqGreen = pow((train.g - tests.g), 2); 
	float sqBlue = pow((train.b - tests.b), 2); 
	float sqPh = pow((train.ph - tests.ph), 2); 
	float euclidean_dist = sqrt(sqRed + sqGreen + sqBlue + sqPh); 
	return euclidean_dist; 
}

int knn(int data_size, struct Point testingPoint, int k) {
	for (int i = 0; i < data_size; i++) {
    distances[i] = euclideanDistances(train_data[i], testingPoint);
    labels[i] = train_data[i].label;
	}
 	for (int i = 0; i < data_size - 1; i++) {
    for (int j = 0; j < data_size - i - 1; j++) {
      if (distances[j] > distances[j + 1]) {
        swapFLTvalue(&distances[j], &distances[j + 1]); 
				swapINTvalue(&labels[j], &labels[j + 1]); 
			}
		}
	}
	for (int i = 0; i < k; i++) {
		counting_labels[labels[i]]++;
	}
	for (int i = 0; i < MAX_DATA; i++) {
		if (counting_labels[i] > max_counting) {
			max_counting = counting_labels[i];
			most_freq_label = i;
		}
	}
	return most_freq_label; 
}
/* 
void runningKNN() {
	Serial.println("K-NN has been starting "); 
  int valueK = K; 
  int train_data_size = sizeof(train_data); 
  
	float featureValRed = (redValue / 255.0); 
	float featureValGreen = (greenValue / 255.0); 
	float featureValBlue = (blueValue / 255.0); 
	float featureValPH = (pHactual / 7.0); 

	testingPoint.r = featureValRed; 
	testingPoint.g = featureValGreen; 
	testingPoint.b = featureValBlue; 
	testingPoint.ph = featureValPH; 
	testingPoint.label = -1;  
	
  // delay(100); 
	Serial.println("Before classification of K-NN "); 
  int classification = knn(train_data, train_data_size, testingPoint, valueK); 	
  Serial.print("Classification Results = "); 
	Serial.print(classification); 
  delay(100); 
	
	switch (classification) {
		case 0 : 
			classification_condition = " WAKTU (0 - 4) JAM "; 
			break; 
		case 1 : 
			classification_condition = " WAKTU (4 - 8) JAM ";
			break; 
		case 2 : 
			classification_condition = " WAKTU ( > 8) JAM "; 
			break; 
    default : 
      classification_condition = "SILAHKAN DICOBA LAGI"; 
      break;  
	}
	Serial.println(classification);
  Serial.println(classification_condition);  
	delay(100); 
} 

void lcdDisplayKNN() { 
	lcd.clear(); 
  lcd.setCursor(0, 2); 
	lcd.print("TIME = ");
	lcd.print(comptime); 
	lcd.setCursor(16, 2); 
	lcd.print(" ms "); 
  Serial.print("TIME = ");
  Serial.print(comptime); 
	Serial.println(" ms "); 
  lcd.setCursor(0, 3); 
	lcd.print(classification_condition); 
  Serial.print(classification_condition); 
} 
*/ 
void lcdDisplayRoutine() { 
	lcd.clear(); 
  lcd.setCursor(0, 0); 
  lcd.print("TESTING COUNTING");
  // lcd.print(count);
  Serial.println("TESTING COUNTING"); 
  // Serial.println(count); 
  
  lcd.setCursor(0, 1); 
  lcd.print("LOOP COUNT = "); 
  lcd.print(count); 
  Serial.print("LOOP COUNT = "); 
  Serial.println(count);  
} 

void setup() { 
	Serial.begin(9600); 
	// pinMode(PIN, OUTPUT); 
	pinMode(BUTTON, INPUT); 
  // int valueK = K;  
	// int train_data_size = sizeof(train_data); 
  delay(100); 
	lcd.init();
	lcd.backlight();
	lcd.clear(); 
	lcd.setCursor(0, 0);
	lcd.print(" LCD INITIALIZATION ");
	delay(2000); 
	lcd.clear();  
  
  float data_size = (float) (sizeof(train_data) / sizeof(train_data[0]));  
  const int valueK = K; 
  const int train_data_size = (int) data_size; 
  // const int train_data_size = 48;   

  Serial.print("Value of K = ");
  Serial.println(valueK);  
  Serial.print("Data Size of Training Data = "); 
  Serial.println(train_data_size); 
  Serial.print("Most Frequent Label = ");
  Serial.println(most_freq_label); 
  delay(100); 

  float featureRED = (redValue / 255.0); 
  float featureGREEN = (greenValue / 255.0); 
  float featureBLUE = (blueValue / 255.0); 
  float featurePH = (pHactual / 7.00); 
  Point testingPoint = {featureRED, featureGREEN, featureBLUE, featurePH, -1};  
  delay(100);   

  Serial.println(featureRED); 
  Serial.println(featureGREEN); 
  Serial.println(featureBLUE); 
  Serial.println(featurePH); 
  delay(1000);  
}  

void loop() {
	// comptime = millis();
	// state = false; 
  // delay(100); 
  // int classification; 
  // const int valueK = K; 
  // const int train_data_size = 12; 
  float data_size = (float) (sizeof(train_data) / sizeof(train_data[0]));  
  const int valueK = K; 
  const int train_data_size = (int) data_size; 
  int classification; 
  delay(100); 
  
  if (state == true) { 
    Serial.println("Starting the K-NN classification "); 
    delay(100); 
    classification = knn(train_data_size, testingPoint, valueK); 	
    Serial.print("Classification Results = "); 
	  Serial.println(classification); 
    Serial.print("Most Frequent Label = ");
    Serial.println(most_freq_label); 
    delay(100); 

    // runningKNN(); 
    // delay(100); 
    // lcdDisplayKNN();
    // delay(1000); 
  } 
  
	button_state = digitalRead(BUTTON);	  // read the state of the pushbutton value
  Serial.println(button_state); 	      // check if pushbutton is not pressed.  if it is not, the
  delay(100); 
  if (button_state == HIGH) {			      // button_state is HIGH (tombol ditekan)
  	state = true;   
    Serial.println("Start K-NN"); 
    // delay(100); 
    // runningKNN(); 
  } else if (button_state == LOW) { 	  // button_state is LOW  (tombol dilepas)
    if (state == false) { 
      lcdDisplayRoutine(); 
      delay(100); 
    } 
    Serial.println("Just Counting"); 
	}
  delay(100); 						// Delay a little bit to improve simulation performance 
  Serial.println("Program is still Running"); 	
  count++; 
  // lcd.clear(); 
  delay(1000); 
}