/**
 * The next sketch is a part of a bigger project related to dressing
 * The setup is described in the instructable and it be able to scan clothes colors using handrawn buttons + Blynk App
 * to fold the unwanted clothes!
 * Once a button is pressed, the CPX will scan the color that is next to it and store the color it received (using HSV transformation) in the
 * associated wear.
 * Once the data is sent to Blynk, the scenario will analyze the color and send a push notification of weather the clothes colors fit or not.
 * 
 * The circuit: Circuit Playground Express + ESP8266 + 5 Alligator cables connected to handrawn buttons
 * 
 * Created By:
 * Idan Kringel
 * Ori Blanka
 * David German
 */


#include <Adafruit_CircuitPlayground.h>
#include <Wire.h>
#include <SPI.h>
#include <ESP8266_Lib.h>
#include <BlynkSimpleShieldEsp8266.h>
#include "BlynkConfig.h"

#define SHIRT A1
#define SHOES A2
#define PANTS A3
#define JACKET A4
#define BELT A5
#define CWHITE 0
#define CBLACK 1
#define CSILVER 2
#define CRED 3
#define CYELLOW 4
#define CGREEN 5
#define CLIGHTBLUE 6
#define CBLUE 7
#define CPINK 8 

float MINRED = 330.0;
float MAXRED = 30.0; 
float MINYELLOW = 30.0;
float MAXYELLOW = 90.0;
float MINGREEN = 90.0;
float MAXGREEN = 150.0;
float MINLIGHTBLUE = 150.0;
float MAXLIGHTBLUE = 210.0;
float MINBLUE = 210.0;
float MAXBLUE = 270.0;
float MINPINK = 270.0;
float MAXPINK = 330.0;

#define FITAMOUNT 12 
#define CAPACITANCE 5

#define EspSerial Serial1
#define ESP8266_BAUD 115200

char auth[] = "qNYwgySpRJpH0xD3tpzaC-eSngFthPN8"; //My auth token
char ssid[] = "elor10"; //My wifi name
char pass[] = "1122334455"; //My wifi password

ESP8266 wifi(&EspSerial);

long lastBlynk = 0;
 
byte capsensePins[] = {
  SHIRT, SHOES, PANTS, JACKET, BELT
};

int clouthesColor[sizeof(capsensePins)] = {-1, -1, -1, -1, -1};
int fitting[FITAMOUNT][sizeof(capsensePins)] = {{CBLUE, CBLUE, CBLACK, CBLACK, CBLACK},
                                                {CWHITE, CSILVER, CBLACK, CWHITE, CBLACK},
                                                {CBLACK, CRED, CWHITE, CRED, CBLACK},
                                                {CLIGHTBLUE, CWHITE, CBLACK, CGREEN, CWHITE},
                                                {CWHITE, CSILVER, CBLACK, CBLACK, CSILVER},
                                                {CWHITE, CLIGHTBLUE, CPINK, CBLACK, CWHITE},
                                                {CYELLOW, CBLACK, CBLUE, CBLACK, CBLUE},
                                                {CSILVER, CLIGHTBLUE, CWHITE, CBLUE, CBLACK},
                                                {CPINK, CYELLOW, CBLACK, CYELLOW, CBLACK},
                                                {CBLUE, CGREEN, CWHITE, CLIGHTBLUE, CWHITE},
                                                {CBLACK, CPINK, CBLACK, CWHITE, CWHITE},
                                                {CWHITE, CBLUE, CWHITE, CLIGHTBLUE, CBLUE}};

void setup() {
  Serial.begin(9600);
  CircuitPlayground.begin();
  CircuitPlayground.strip.setBrightness(255);
  EspSerial.begin(ESP8266_BAUD);
  Blynk.begin(auth, wifi, ssid, pass);
}

void loop() {
  Serial.println("start");
  Serial.println(sizeof(capsensePins));
  for(int i = 0; i < CAPACITANCE; ++i) {
    Serial.print(i); 
    Serial.print(": "); 
    Serial.print(CircuitPlayground.readCap(capsensePins[i]));
    if(CircuitPlayground.readCap(capsensePins[i]) < 100){
      clouthesColor[i] = colorSense();
      Serial.print(clouthesColor[i]);
    }
  }
  if (millis()-lastBlynk > 1000){
     Blynk.run();
     lastBlynk = millis();
  }
}

BLYNK_WRITE(FIT_CLOUTHES_COLORS_PIN){
  for(int i = 0; i < sizeof(capsensePins); ++i){
    clouthesColor[i] = -1;
  }
  Serial.println("Fitting");
}

BLYNK_WRITE(STOP_DETECTING_COLORS_PIN){
  Serial.println("Stop detect colors");
  boolean fit = fitColors();
  if(fit == true){
    Blynk.virtualWrite(V5, 1);
  } else {
    Blynk.virtualWrite(V5, 0);
  }
  
}

// Detecte color form choosen clouth
int colorSense(){
  Serial.println("colorSense");
  delay(100);
  
  CircuitPlayground.clearPixels();
  // Now take a color reading (the red, green, blue color components will be
  // returned in the parameters passed in).
  uint8_t red, green, blue;
  CircuitPlayground.senseColor(red, green, blue);
  
  // Print out the color components.
  Serial.print("Color: red=");
  Serial.print(red, DEC);
  Serial.print(" green=");
  Serial.print(green, DEC);
  Serial.print(" blue=");
  Serial.println(blue, DEC);
  
  // Gamma correction makes LED brightness appear more linear
  red   = CircuitPlayground.gamma8(red);
  green = CircuitPlayground.gamma8(green);
  blue  = CircuitPlayground.gamma8(blue);
  
  // Finally set all the pixels to the detected color.
  for (int i=0; i<10; ++i) {
    CircuitPlayground.strip.setPixelColor(i, red, green, blue);
  }
  CircuitPlayground.strip.show();

  //change RGB Color Model to HSV Color Mode
  return RGB2HSV(red, green, blue);
}

// Change RGB color model to HSV color model 
int RGB2HSV(uint8_t red, uint8_t green, uint8_t blue){
  float r, g, b;

  r = red / 255.0;
  g = green / 255.0;
  b = blue / 255.0;

  float cmax = max(r, max(g,b));
  float cmin = min(r, min(g,b));
  float diff = cmax - cmin;
  float h = -1;
  float s = -1;

  if(cmax == cmin){
    h = 0;
  } else {
    if (cmax == r){
      h = fmod((60 * ((g-b) / diff) + 360), 360.0);
    } else {
      if (cmax == g){
        h = fmod((60 * ((b-r) / diff) + 120), 360.0);
      } else {
        if (cmax == b){
          h = fmod((60 * ((r-g) / diff) + 240), 360.0);
        }
      }
    }
  }
  if(cmax == 0){
    s = 0;
  } else {
    s = (diff / cmax) * 100;
  }

  double v = cmax * 100;
  Serial.println("H S V");
  Serial.println(h);
  Serial.println(s);
  Serial.println(v);
  return colorDetermination(h, s, v);
}

// Get color in HSV format and detrmine what color is it
int colorDetermination(float h, float s, float v){
  int hue;

  //Determine if the hue is white, gray or black
  if(h == 0.0 && s == 0.0){
    if(v >= 0 && v < 50){
      hue = CBLACK;
    } else if(v >= 75 && v < 100){
       hue = CSILVER;
    } else {
       hue = CWHITE;
    }
  } else if(h >= MINYELLOW && h < MAXYELLOW){ //Determine which hue is the given color is belong to
      hue = CYELLOW;
  } else if (h >= MINGREEN && h < MAXGREEN){
       hue = CGREEN;
  } else if (h >= MINLIGHTBLUE && h < MAXLIGHTBLUE){
      hue = CLIGHTBLUE;
  } else if (h >= MINBLUE && h < MAXBLUE){
      hue = CBLUE;
  } else if(h >= MINPINK && h < MAXPINK){
      hue = CPINK;
  } else if(h >= MINRED && h <= 360 && h >= 0 && h < MAXRED){
      hue = CRED;
  }
  
  Serial.println(hue);
  return hue;
}

// Get clouthes array and return if the chosen color are looks good togther
boolean fitColors(){
  int i,j;
  for(i = 0; i < FITAMOUNT; i++){
    for(j = 0; j < sizeof(capsensePins); j++){
      if(clouthesColor[j] != fitting[i][j] && clouthesColor[j] != -1){
        break;
      }
    }
    if(j == sizeof(capsensePins)){
      return true;
    }
  }
  return false;
}
