/**
   "Crown Jewels" Escape Room Puzzle, (c) 2023 Playful Technology
   Object detection using multiple TCS34725 colour sensors
   via a TCA9548A I2C multiplexer
*/

// DEFINES
#define ARRAY_SIZE(x) ((sizeof x) / (sizeof *x))

// INCLUDES
// Built-in Arduino library for addressing I2C devices
#include <Wire.h>
// For addressing PL9823 progammable RGB LEDs. See http://fastled.io/
#include <FastLED.h>
// For interfacing with TCS34725 colour sensor. See https://github.com/playfultechnology/TCS34725
#include "TCS34725.h"

// STRUCTS AND ENUMS
// Enumerate each of the possible objects that we want to detect
enum Object {None, Ruby, Sapphire};
// Define a struct to record colour data about each object:
// - "input" RGB and lux component values as recorded by the sensor,
// - "output" RGB values that should be sent to the LED strip
struct Colour {
  TCS34725::Color inRGB; // RGB values as detected by TCS34725
  float lux; // Input lux as detected by TCS34725
  CRGB outRGB; // RGB values to be emitted by PL9823 LEDs
};

// CONSTANTS
// Array to associate each object with its appropriate colour values
// The .inRGB input values were recorded by holding each item up to the sensor and averaging
// the values printed in the serial monitor
const Colour colours[] = {
  [None] = { .inRGB = {14, 19, 14}, .lux = 550, .outRGB = {255, 255, 255}},
  [Ruby] = { .inRGB = {141, 1, 2}, .lux = 55, .outRGB = {255, 0, 0} },
  [Sapphire] = { .inRGB = {1, 24, 44}, .lux = 140, .outRGB = {0, 0, 255}}
};
// Address of TCA9548A multiplexer set by setting A0-A2 lines HIGH/LOW
// LLL=0x70 (default), HLL=0x71, LHL=0x72, HHL=0x73, LLH=0x74, HLH=0x75, LHH=0x76, HHH=0x77
const byte multiplexAddress = 0x70;
const byte numSensors = 1;
const byte ledDataPin = 15; // GPIO pin connected to DataIn of first LED
const byte relayPin = 14; // GPIO pin connected to signal line of relay module
// The objects that should be placed in front of each sensor to solve the puzzle
const Object targetObjects[numSensors] = { Ruby};
// Defines how similar detected colour must be to target colour to be matched
const int threshold = 700;

// GLOBALS
// Initialise an array of TCS sensor objects
TCS34725 tcs[numSensors] = {TCS34725()};
// An array to record the detected colours from each sensor
Object detectedObjects[numSensors] = { (Object)-1};
// Define the array of LED RGB values - one per sensor
CRGB leds[numSensors];

// FUNCTIONS
// Select the appropriate channel from the I2C multiplexer
void selectI2CBus(uint8_t channel) {
  Wire.beginTransmission(multiplexAddress);
  Wire.write(1 << channel);
  Wire.endTransmission();
}

// Test the similarity between two colours by the difference in their R,G,B, and Lux components
uint32_t getColourDistance(TCS34725::Color Col1, float lux, Colour Col2) {
  return (abs(Col1.r - Col2.inRGB.r) + abs(Col1.g - Col2.inRGB.g) + abs(Col1.b - Col2.inRGB.b) + abs(Col2.lux - lux));
}

void setup(void) {
  // Serial interface used for calibration and debugging
  Serial.begin(115200);
  Serial.println(__FILE__ __DATE__);

  for( int i =0; i < ARRAY_SIZE(colours); ++i) {
    Colour c = colours[i];
    TCS34725::Color in = c.inRGB;
    CRGB out = c.outRGB;
    Serial.print("i="); Serial.print(i);
    Serial.print(", inRGB="); Serial.print(in.r);
    Serial.print(","); Serial.print(in.g);
    Serial.print(","); Serial.print(in.b);
    Serial.print(", lux="); Serial.print(c.lux);
    Serial.print(", outRGB="); Serial.print(out.r); 
    Serial.print(","); Serial.print(out.g); 
    Serial.print(","); Serial.println(out.b); 
  }
  // Start the I2C interface
  Wire.begin();

  // Configure relay to maglock
  pinMode(relayPin, OUTPUT);
  digitalWrite(relayPin, HIGH);

  // Initialise the array of colour sensors
  for (int i = 0; i < numSensors; i++) {
    Serial.print(F("Initialising sensor #"));
    Serial.print(i);
    // Set the multiplexer to the appropriate channel
    selectI2CBus(i);
    // Configure sensor
    if (tcs[i].attach(Wire)) {
      tcs[i].integrationTime(500); // ms
      tcs[i].gain(TCS34725::Gain::X01);
      Serial.println(F(" OK!"));
    }
    else {
      Serial.println(F(" Failed to initialise TCS34725"));
    }
    delay(100);
  }

  // Initialise the LEDs
  FastLED.addLeds<PL9823, ledDataPin, RGB>(leds, numSensors).setCorrection(TypicalLEDStrip);
}

void loop(void) {

  // Loop over every sensor to see if a new object has been detected
  bool hasChanged = false;
  for (int i = 0; i < numSensors; i++) {
    // Select the appropriate channel on the I2C multiplexer
    selectI2CBus(i);
    // If there is data available to read
    if (tcs[i].available()) {
      // Retrieve the colour and lux values detected by the sensor
      TCS34725::Color colour = tcs[i].color();
      float lux = tcs[i].lux();

      // DEBUG/CALIBRATION Print sensor values on the serial monitor
      char buffer[50];
      snprintf(buffer, 50, "Sensor #%d, %.f, %.f, %.f, %.f", i, colour.r, colour.g, colour.b, tcs[i].lux());
      Serial.println(buffer);

      // What was the last known object detected by this sensor?
      Object previousDetectedObject = detectedObjects[i];
      Serial.print("PreviousDetectedObject="); Serial.println((int)detectedObjects[i]);

      // Compare sensor readings to known colours
      uint32_t currentMinDelta = threshold;
      for (int j = 0; j < ARRAY_SIZE(colours); j++) {
        // Calculate the difference between the detected colour and the correct target colour
        uint32_t delta = getColourDistance(colour, lux, colours[j]);
          Serial.print("delta = "); Serial.println(delta);
        // If this is more similar than the previous best match
        if (delta < currentMinDelta) {
          // Update the best score
          currentMinDelta = delta;
          // Update the array of detected objects
          detectedObjects[i] = (Object)j;
          Serial.print("updating detectedObjects to "); Serial.println(j);
          delay(100);
        }
      }

      // If the detected object is different to the one previously known
      if (detectedObjects[i] != previousDetectedObject) {
        hasChanged = true;
        Serial.println("Change detected");
      }
      else {
        Serial.print("i="); Serial.print(i);
        Serial.print(", detectedObjects[i] ="); Serial.print((int)detectedObjects[i]);
        Serial.print(", previousDetectedObject =");Serial.println(previousDetectedObject);
      }
    }
  }

  // If the colour detected by at least one sensor has changed
  if (hasChanged) {
    // Check to see if puzzle is solved
    bool allCorrect = true;
    for (int i = 0; i < numSensors; i++) {
      // Debug - print an array of the objects detected by each sensor
      Serial.print(detectedObjects[i]);
      if (i < numSensors - 1) {
        Serial.print(",");
      }
      else {
        Serial.println("");
      }

      // Set corresponding LED colour for each sensor
      leds[i] = colours[detectedObjects[i]].outRGB;

      // If any sensor detects an object different from that in the targetObjects array, the puzzle is not solved
      if (detectedObjects[i] != targetObjects[i]) {
        allCorrect = false;
      }
    }
    // If we have made it this far and allCorrect is still true, release the maglock
    if (allCorrect) {
      digitalWrite(relayPin, LOW);
      Serial.println ("correct");
    }
    else {
      digitalWrite(relayPin, HIGH);
    }

    // Update the LEDs
    FastLED.show();
  }
  delay(500);
  Serial.println("");
}