/*
    Description: Use COLOR Unit to read C, R, G, B values and serial print output.
    Please install library before compiling:  
    Adafruit TCS34725: https://github.com/adafruit/Adafruit_TCS34725
*/
#include <Wire.h>
#include <M5Stack.h>
#include "Adafruit_TCS34725.h"



// set to false if using a common cathode LED
#define commonAnode true

// our RGB -> eye-recognized gamma color
byte gammatable[256];

static uint16_t color16(uint16_t r, uint16_t g, uint16_t b) {
	uint16_t _color;
	_color = (uint16_t)(r & 0xF8) << 8;
	_color |= (uint16_t)(g & 0xFC) << 3;
	_color |= (uint16_t)(b & 0xF8) >> 3;
  return _color;
}

Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X);

void setup() {
  delay(100);

  M5.begin(true, false, false);
  M5.Power.begin();
  Serial.begin(115200);
  Serial.println("Color View Test!");
  while(!tcs.begin()){
      Serial.println("No TCS34725 found ... check your connections");
      M5.Lcd.setTextFont(4);
      M5.Lcd.setTextColor(TFT_WHITE, TFT_BLACK);
      M5.Lcd.drawString("No Found sensor.",50, 100, 4);
      delay(1000);
  }
  tcs.setIntegrationTime(TCS34725_INTEGRATIONTIME_154MS);
  tcs.setGain(TCS34725_GAIN_4X);
}

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


  delay(60);  // takes 50ms to read 
  
  tcs.getRawData(&red, &green, &blue, &clear);

  
  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);

  // Figure out some basic hex code for visualization
  uint32_t sum = clear;
  float r, g, b;
  r = red; r /= sum;
  g = green; g /= sum;
  b = blue; b /= sum;
  r *= 256; g *= 256; b *= 256;
  Serial.print("\t");
  Serial.print((int)r, HEX); Serial.print((int)g, HEX); Serial.print((int)b, HEX);
  Serial.println();
  uint16_t _color = color16((int)r, (int)g, (int)b);
  M5.Lcd.clear(_color);
}
