#include <Servo.h>
//NON YOWARE
#define N_SAMPLES 20 // replace 100 with the number of samples in your data
unsigned long startMillis;
unsigned long currentMillis;
const unsigned long period = 1000;  //the value is a number of milliseconds
float emgData[N_SAMPLES] = {45,43,48,152,437,440,362,363,338,364,329,363,326,316,244,150,97,72,56,47,};
bool isMoving[5] = {false, false, false, false, false};
int dataIndex = 0;
//NON MYOWARE
Servo jempol;
Servo telunjuk;
Servo tengah;
Servo manis;
Servo kelingking;

const int EMG_PIN = A2;
const float ALPHA = 0.5;
float filteredValue = 0;

int varCount = 0;
float varSum = 0;
float varSumSq = 0;

#define N_SAMPLES 140
#define N_FEATURES 1
#define K 3
//Waveform Length
float trainingData[N_SAMPLES][N_FEATURES] = {
  {71.74},{68.25},{65.94},{82.79},{76.21},{85.72},{79.01},{87.30},{66.71},{61.67},{70.66},{63.63}, // Cool
  {107.86},{87.74},{105.04},{105.13},{102.41},{104.80},{89.19},{103.60},{107.34},{105.65},{103.54},{101.45}, // Spiderman
  {140.13},{190.99},{176.02},{157.66},{182.43},{163.77},{190.66},{150.66},{166.75},{141.43},{164.33},{140.57},// Sip
  {228.54},{217.73},{261.08},{270.06},{262.37},{225.54},{221.39},{242.78},{260.54},{278.44},{279.36},{201.34}, // Genggam
  {57.63},{57.06},{49.34},{51.19},{73.92},{62.17},{66.25},{56.41},{57.24},{49.99},{59.15},{56.04},// Tengah
  {50.87},{44.34},{42.58},{40.48},{45.03},{45.00},{44.55},{44.63},{44.56},{45.48},{45.85},{45.45},// Oke Tengah
  {110.47},{120.91},{126.80},{108.11},{120.81},{131.55},{122.01},{113.71},{114.04},{114.59},{118.32},{123.87},// Pew Pew
};

int trainingLabels[N_SAMPLES] = {
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // Cool
  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // Spiderman
  2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // Sip
  3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3,// Genggam
  4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4,// Tengah
  5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,//Oke Tengah
  6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,//Pew Pew
};

int classify(float testData[N_FEATURES]) {
  float closestDists[K];
  int closestLabels[K];

  // Initialize the arrays with large distances and invalid labels
  for (int i = 0; i < K; i++) {  
    closestDists[i] = 1000000;
    closestLabels[i] = -1;
  }

  for (int i = 0; i < N_SAMPLES; i++) {
    float dist = 0;
    for (int j = 0; j < N_FEATURES; j++) {
      dist += abs(testData[j] - trainingData[i][j]);
    }

    // Insert the new distance into the correct position in the array
    for (int k = 0; k < K; k++) {
      if (dist < closestDists[k]) {
        // Shift the existing distances and labels to make room for the new one
        for (int l = K-1; l > k; l--) {
          closestDists[l] = closestDists[l-1];
          closestLabels[l] = closestLabels[l-1];
        }
        // Insert the new distance and label
        closestDists[k] = dist;
        closestLabels[k] = trainingLabels[i];
        break;
      }
    }
  }

  // Count the occurrences of each label among the K closest neighbors
  int labelCounts[K] = {0};
  for (int i = 0; i < K; i++) {
    labelCounts[closestLabels[i]]++;
  }

  // Find the label with the most occurrences
  int mostCommonLabel = -1;
  int maxCount = -1;
  for (int i = 0; i < K; i++) {
    if (labelCounts[i] > maxCount) {
      maxCount = labelCounts[i];
      mostCommonLabel = i;
    }
  }

  return mostCommonLabel;
}

void setup() {
  Serial.begin(9600);
  //non
  startMillis = millis();
  //non
  jempol.attach(8);
  telunjuk.attach(9);
  tengah.attach(10);
  manis.attach(11);
  kelingking.attach(12);
}

void loop() {
  // int emgValue = analogRead(EMG_PIN);
  //NON MYOWARE 
  float emgValue = emgData[dataIndex];
//NON MYOWARE
  filteredValue = ALPHA * emgValue + (1 - ALPHA) * filteredValue;

 Serial.println(filteredValue);
  varSum += filteredValue;
  varSumSq += filteredValue * filteredValue;
  varCount++;
    float mean = varSum / varCount;
    float variance = varSumSq / varCount - mean * mean;
    Serial.print("Variance: ");
    Serial.println(variance);
    if (variance <= 20) {
  for (int i = 0; i < 5; i++) {
    if (isMoving[i]) {
      // Code to revert servo i to neutral position
      if (i == 0) { // jempol
      for (int pos = 180; pos >= 0; pos -= 1) {
        jempol.write(pos);
      }
      } else if (i == 1) { // telunjuk
        for (int pos = 180; pos >= 0; pos -= 1) {
        telunjuk.write(pos);
      }
      } else if (i == 2) { // tengah
      for (int pos = 180; pos >= 0; pos -= 1) {
        tengah.write(pos);
      }
      } else if (i == 3) { // manis
      for (int pos = 180; pos >= 0; pos -= 1) {
        manis.write(pos);
      }
      } else if (i == 4) { // kelingking
      for (int pos = 180; pos >= 0; pos -= 1) {
        kelingking.write(pos);
      }
      }
    }
  }
  // Reset the isMoving array
  for (int i = 0; i < 5; i++) {
    isMoving[i] = false;
  }
}
  else {
   float testData[N_FEATURES] = {variance};
        int predictedLabel = classify(testData);
    if (predictedLabel == 0) {
      for (int pos = 0; pos <= 180; pos += 1) {
        telunjuk.write(pos);
        tengah.write(pos);
        manis.write(pos);
        delay(15);
      }
      isMoving[1] = true; 
      isMoving[2] = true; 
      isMoving[3] = true; 
    }
    else if (predictedLabel == 1) {
        for (int pos = 0; pos <= 180; pos += 1) {
        tengah.write(pos);
        manis.write(pos);
        delay(15);
      }
      isMoving[2] = true; 
      isMoving[3] = true; 
    }
    else if (predictedLabel == 2) {
        for (int pos = 0; pos <= 180; pos += 1) {
        telunjuk.write(pos);
        tengah.write(pos);
        manis.write(pos);
        kelingking.write(pos);
        delay(15);
      }
      isMoving[1] = true; 
      isMoving[2] = true; 
      isMoving[3] = true;
      isMoving[4] = true; 
    }
    else if (predictedLabel == 3) {
        for (int pos = 0; pos <= 180; pos += 1) {
        jempol.write(pos);
        telunjuk.write(pos);
        tengah.write(pos);
        manis.write(pos);
        kelingking.write(pos);
        delay(15);
      }
      isMoving[0] = true;
      isMoving[1] = true; 
      isMoving[2] = true; 
      isMoving[3] = true;
      isMoving[4] = true; 
    }
    else if (predictedLabel == 4) {
        for (int pos = 0; pos <= 180; pos += 1) {
        tengah.write(pos);
        delay(15);
      }
      isMoving[2] = true;
    }
    else if (predictedLabel == 5) {
        for (int pos = 0; pos <= 180; pos += 1) {
        jempol.write(pos);
        telunjuk.write(pos);
        tengah.write(pos);
        delay(15);
      }
      isMoving[0] = true;
      isMoving[1] = true; 
      isMoving[2] = true; 
    }
    else if (predictedLabel == 6) {
        for (int pos = 0; pos <= 180; pos += 1) {
        tengah.write(pos);
        manis.write(pos);
        kelingking.write(pos);
        delay(15);
      }
      isMoving[2] = true; 
      isMoving[3] = true;
      isMoving[4] = true; 
    }
  }
  // Reset MAV variables
   varCount = 0;
    varSum = 0;
    varSumSq = 0;
    dataIndex = (dataIndex + 1) % N_SAMPLES;
  unsigned long currentMillis = millis();  //get the current "time" (actually the number of milliseconds since the program started)
  Serial.println(currentMillis);  //print the time
  delay(1000);
  delay(100);
  // Delay MAV
}