#include <Wire.h>
#include "Adafruit_TCS34725.h"
#include <Adafruit_NeoPixel.h>
#include "Flora_Pianoglove.h"

#define CLEARTHRESHHOLD 2000





// RGB값을 '눈'이 인식가능한 감마값으로 변환합니다.
byte gammatable[256];



// 컬러센서에 사용된 TCS34725를 사용준비합니다.
Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X);
// 네오픽셀을 6번핀에 연결합니다.
Adafruit_NeoPixel strip = Adafruit_NeoPixel(1, 6, NEO_GRB + NEO_KHZ800);

 // TX만 사용합니다.

void setup() {
  
  Serial.begin(9600);


  //컬러센서가 준비되면, 시리얼모니터상에 found sensor를 전송합니다.
  if (tcs.begin()) {
    Serial.println("Found sensor");
  } 
  else {
    Serial.println("No TCS34725 found ... check your connections");
    while (1); 
  }

  //컬러 재생을 위해 초기화를 실행 합니다.
  strip.begin();
  strip.show(); // 모든 네오픽셀을 꺼짐 상태로 만듭니다.



  // 사람이 인지할수있는 감마값으로 색정보를 변홥 합니다.
  for (int i=0; i<256; i++) {
    float x = i;
    x /= 255;
    x = pow(x, 2.5);
    x *= 255;

    gammatable[i] = x;      

  }
}


void loop() {
  uint16_t clear, red, green, blue;

  tcs.setInterrupt(false);      // LED를 켭니다.

  delay(60);  //50밀리초동안 읽습니다.

  tcs.getRawData(&red, &green, &blue, &clear);

  tcs.setInterrupt(true);  // LED를 끕니다.

  //충분한 색정보를 불러올 수 있는 대상이 없을 경우
  if (clear < CLEARTHRESHHOLD) {
    strip.setPixelColor(0, strip.Color(0, 0, 0)); // LED를 끕니다.
    strip.show();
    return;
  }

  //  Serial.print("C:\t"); Serial.print(clear);
  //  Serial.print("\tR:\t"); Serial.print(red);
  //  Serial.print("\tG:\t"); Serial.print(green);
  //  Serial.print("\tB:\t"); Serial.print(blue);

  // 시각화를 위해 hex code를 계산합니다.
  uint32_t sum = red;
  sum += green;
  sum += blue;
  sum = clear;
  float r, g, b;
  r = red; 
  r /= sum;
  g = green; 
  g /= sum;
  b = blue; 
  b /= sum;
  r *= 256; 
  g *= 256; 
  b *= 256;
  if (r > 255) r = 255;
  if (g > 255) g = 255;
  if (b > 255) b = 255;

  //  Serial.print("\t");
  //  Serial.print((int)r, HEX); Serial.print((int)g, HEX); Serial.print((int)b, HEX); 
  //  Serial.println();



  float remove, normalize;
  if ((b < g) && (b < r)) {
    remove = b;
    normalize = max(r-b, g-b);
  } 
  else if ((g < b) && (g < r)) {
    remove = g;
    normalize = max(r-g, b-g);
  } 
  else {
    remove = r;
    normalize = max(b-r, g-r);
  }
  //작은 소수를 제거합니다.
  float rednorm = r - remove;
  float greennorm = g - remove;
  float bluenorm = b - remove;
  // 가장 높은수를 정상화 합니다.
  rednorm /= normalize;
  greennorm /= normalize;
  bluenorm /= normalize;

  //  Serial.println();
  strip.setPixelColor(0, strip.Color(gammatable[(int)r], gammatable[(int)g], gammatable[(int)b]));
  strip.show();

  //  Serial.print(rednorm); Serial.print(", "); 
  //  Serial.print(greennorm); Serial.print(", "); 
  //  Serial.print(bluenorm); Serial.print(" "); 
  //  Serial.println();

}




RgbColor HsvToRgb(HsvColor hsv){
  
  RgbColor rgb;
  unsigned char region, remainder, p, q, t;

  if (hsv.s == 0)
  {
    rgb.r = hsv.v;
    rgb.g = hsv.v;
    rgb.b = hsv.v;
    return rgb;
  }

  region = hsv.h / 43;
  remainder = (hsv.h - (region * 43)) * 6; 

  p = (hsv.v * (255 - hsv.s)) >> 8;
  q = (hsv.v * (255 - ((hsv.s * remainder) >> 8))) >> 8;
  t = (hsv.v * (255 - ((hsv.s * (255 - remainder)) >> 8))) >> 8;

  switch (region)
  {
  case 0:
    rgb.r = hsv.v; 
    rgb.g = t; 
    rgb.b = p;
    break;
  case 1:
    rgb.r = q; 
    rgb.g = hsv.v; 
    rgb.b = p;
    break;
  case 2:
    rgb.r = p; 
    rgb.g = hsv.v; 
    rgb.b = t;
    break;
  case 3:
    rgb.r = p; 
    rgb.g = q; 
    rgb.b = hsv.v;
    break;
  case 4:
    rgb.r = t; 
    rgb.g = p; 
    rgb.b = hsv.v;
    break;
  default:
    rgb.r = hsv.v; 
    rgb.g = p; 
    rgb.b = q;
    break;
  }

  return rgb;
}


HsvColor RgbToHsv(RgbColor rgb){
  
  HsvColor hsv;
  unsigned char rgbMin, rgbMax;

  rgbMin = rgb.r < rgb.g ? (rgb.r < rgb.b ? rgb.r : rgb.b) : 
  (rgb.g < rgb.b ? rgb.g : rgb.b);
  rgbMax = rgb.r > rgb.g ? (rgb.r > rgb.b ? rgb.r : rgb.b) : 
  (rgb.g > rgb.b ? rgb.g : rgb.b);

  hsv.v = rgbMax;
  if (hsv.v == 0)
  {
    hsv.h = 0;
    hsv.s = 0;
    return hsv;
  }

  hsv.s = 255 * long(rgbMax - rgbMin) / hsv.v;
  if (hsv.s == 0)
  {
    hsv.h = 0;
    return hsv;
  }

  if (rgbMax == rgb.r)
    hsv.h = 0 + 43 * (rgb.g - rgb.b) / (rgbMax - rgbMin);
  else if (rgbMax == rgb.g)
    hsv.h = 85 + 43 * (rgb.b - rgb.r) / (rgbMax - rgbMin);
  else
    hsv.h = 171 + 43 * (rgb.r - rgb.g) / (rgbMax - rgbMin);

  return hsv;
}