/*
  In deze schets gaan we met de Arduino BLE Sense op basis van kleur objecten detecteren.
  In deze schets kan je zelf de hoeveelheid objecten aanpassen
  en zelf de hoeveelheid voorbeelden de arduino moet zien voordat je objecten kan detecteren.
*/

//Bibliotheken:
#include <Arduino_KNN.h>
#include <Arduino_APDS9960.h>

//Variabelen:
const int INPUTS = 3;           //Hier definiëren we hoeveel kleur inputs we willen. In dit geval 3, RGB (Red, Green, Blue).
int OBJECTEN = 0;               //Aantal objecten wat we gaan definiëren.
const int VOORBEELDEN = 30;     //Hoeveelheid voorbeelden er worden geregistreerd per object voor referentiemateriaal.
const int K = 5;
String label[3000];             //Hier worden de objectlabels gedefinieert
float color[INPUTS];            //In deze array worden de kleuren opgeslagen.
const int DREMPEL = 5;          //Drempel voor de kleur helderheid.

//Creëer een KNN classifier.
KNNClassifier myKNN(INPUTS);

void setup() {
  pinMode(LED_BUILTIN,OUTPUT);
  digitalWrite(LED_BUILTIN,HIGH);
  
  //Initieer seriële monitor
  Serial.begin(9600);
  while (!Serial);
  
  //initieer APDS (Lichtsensor)
  if (!APDS.begin()) {
    Serial.println("Failled to initialized APDS!");
    while (1);
  }
  
  //Begin Seriele commando's
  Serial.println("Welkom bij de object detectie schets!");
  Serial.println();
  
  //Definieer de hoeveelheid objecten
  Serial.println("Hoeveel objecten wil jij detecteren?");
  Serial.println();
  while(!Serial.available()){}
  delay(100);
  String readstring = Serial.readString();
  OBJECTEN = readstring.toInt();
  Serial.flush();
  Serial.print("Jij hebt ");
  Serial.print(OBJECTEN);
  Serial.println(" objecten!");
  
  digitalWrite(LED_BUILTIN,LOW);
  
  //Definiëer de objecten
  for(int HuidigObject = 0; HuidigObject < OBJECTEN; HuidigObject++){
    Serial.print("Benoem object ");
    Serial.print(HuidigObject);
    Serial.println(":");
  
    while(!Serial.available()){}
    delay(100);
    label[HuidigObject] = Serial.readString();
    Serial.flush();
    Serial.print("Object ");
    Serial.print(HuidigObject);
    Serial.print(" = ");
    Serial.println(label[HuidigObject]);
    
    //Vraag voor voorbeelden van het object
    for (int HuidigVoorbeeld = 0; HuidigVoorbeeld < VOORBEELDEN; HuidigVoorbeeld++) {

      Serial.print("Laat een voorbeeld zien van: ");
      Serial.println(label[HuidigObject]);

      //Wacht voor een object en lees de RGB
      readColor(color);

      //Voeg kleur toe aan het k-NN model
      myKNN.addExample(color, HuidigObject);
    }
    
    Serial.println();
    Serial.print("Object ");
    Serial.print(HuidigObject);
    Serial.print(": ");
    Serial.print(label[HuidigObject]);
    Serial.println(" klaar!");
    Serial.println();
    
    //Wacht totdat het object weer weg is.
    while (!APDS.proximityAvailable() || APDS.readProximity() == 0) {}
  }
  Serial.println("Initieer process klaar!");
  Serial.println();
}

void loop() {
  //Variabelen:
  int classificatie;

  //Wacht totdat het object weer weg is voordat een volgende lezing word uitgevoerd.
  while (!APDS.proximityAvailable() || APDS.readProximity() == 0) {}
  
  Serial.println();
  Serial.println("Laat mij je object raden!");
  Serial.println();

  //Wacht voor een object en lees de kleur.
  readColor(color);

  //Classificeer het object
  classificatie = myKNN.classify(color, K);

  // Print the classification
  Serial.println();
  Serial.print("Jij hebt me een ");
  Serial.print(label[classificatie]);
  Serial.println(" laten zien!");
  Serial.println();
}

void readColor(float color[]) {
  int red, green, blue, proximity, colorTotal = 0;

 //Wacht totdat het object dichtbij komt.
 while (!APDS.proximityAvailable() || APDS.readProximity() > 0){}

  //Wacht totdat de kleur helder genoeg is.
  while (colorTotal < DREMPEL) {

    //Lees als object dichtbij is en helderheid hoog genoeg is.
    if (APDS.colorAvailable()) {

      //Lees kleur en afstand
      APDS.readColor(red, green, blue);
      colorTotal = (red + green + blue);
    }
  }

  //Definieer de kleur in de color array
  color[0] = (float)red / colorTotal;
  color[1] = (float)green / colorTotal;
  color[2] = (float)blue / colorTotal;

  //Print de color array
  Serial.print(color[0]);
  Serial.print(",");
  Serial.print(color[1]);
  Serial.print(",");
  Serial.println(color[2]);
}