// INCLUDES
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#include <SPI.h> // Generic SPI interface, which is used to access the card reader

//Defines
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1  // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define CALIBRATE

// CONSTANTS
const byte chipSelectPin = 8;
// Define the Arduino pins to which the sensor is attached
// S0 and S1 control the output frequency scaling
const byte S0 = 5;
const byte S1 = 4;
// S2 and S3 control the selection of the photodiode filters
const byte S2 = 7;
const byte S3 = 6;
// Output from the sensor is a square wave with 50% duty cycle
const byte sensorOut = 8;
// Calibration values - these need to be obtained by running the sketch in the environment and ambient lighting in 
// which it is to be used, and holding up white and black sample swatches - then copying down the values displayed
// in the serial monitor window to here:
const int32_t blackReading[3] = {0, 0, 0};//2272,2486,21, 1948,2756,24
const int32_t whiteReading[3] = {255, 255, 255}; //281,291,269 //355,372,342

// GLOBALS

// The raw sensor readings (the wave frequency) obtained from selecting the red, green, and blue filtered diodes
unsigned long currentReading[3] = {0, 0, 0};
// The RGB colour value calculated from that sensor reading expressed as a 3-byte array
byte colour[3] = {};
//variables for sine wave

bool puzzleState;
char* displayValue;


//UDF
void showDisplay(){

  Serial.println("display");
  display.clearDisplay();
  display.setTextSize(2);
  display.setCursor(0, 10);
  display.print("Artifact");
  display.setCursor(0,30);
  display.print("ID:");
  display.setCursor(50,30);
  display.print(displayValue);
  display.display();
  delay(2000);
}


void setup() {
  // Initialise serial monitor to be able to read output values
  Serial.begin(9600);

  // Initialise the pins
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(sensorOut, INPUT);
  // The S0 and S1 signals are used to select the output frequency scaling
  // From the datasheet, LL=Power down, LH=2%, HL=20%, HH=100%
  // Scaling the frequency will make the pulses longer, which might lead to more accurate reading, but will also slow down the code 
  digitalWrite(S0,HIGH);
  digitalWrite(S1,LOW);

  // Initialise the display
    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.clearDisplay();
  display.setTextSize(1);      // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE); // Draw white text
  display.setCursor(0, 0);     // Start at top-left corner
  display.print("Initializing...");
  display.display();
  delay(2000);
  display.clearDisplay();
  displayValue = 0;
}

void loop() {
  // Setting S2 Low, S3 Low applies RED filtered photodiodes
  digitalWrite(S2, LOW);
  digitalWrite(S3, LOW);
  // Read the output frequency
  currentReading[0] = pulseIn(sensorOut, digitalRead(sensorOut) == HIGH ? LOW : HIGH);

  // Setting S2 High, S3 High applies GREEN filtered photodiodes
  digitalWrite(S2, HIGH);
  digitalWrite(S3, HIGH);
  // Reading the output frequency
  currentReading[1] = pulseIn(sensorOut,  digitalRead(sensorOut) == HIGH ? LOW : HIGH);

  // Setting S2 LOW, S3 HIGH applied BLUE filtered photodiodes
  digitalWrite(S2, LOW);
  digitalWrite(S3, HIGH);
  // Reading the output frequency
  currentReading[2] = pulseIn(sensorOut,  digitalRead(sensorOut) == HIGH ? LOW : HIGH);

  #ifdef CALIBRATE
    char rawOutput[32];
    snprintf(rawOutput, 32, "Raw sensor values: %lu,%lu,%lu" , currentReading[0], currentReading[1], currentReading[2]);
    Serial.println(rawOutput);
  #endif

  // Loop over each of the three measurements taken this frame 
  for (int i=0; i<3; i++) {
    // Use map() function to rescale values to the range of readings taken for white/black extremes
    int x = map(currentReading[i], blackReading[i], whiteReading[i], 0, 255);
    // Copy result back into the corresponding component of the RGB "colour" variable
    colour[i] = (byte)constrain(x, 0, 255);
  }

  // Send the RGB values to the serial monitor
  char output[32];
  snprintf(output, 32, "(%i,%i,%i)" , colour[0], colour[1], colour[2]);
  Serial.println(output);

  // The following code demonstrates how to perform some action if the colour detected
  // is similar enough to a specified target colour. First, specify the RGB values of the target
  int targetColourAR = 50, targetColourAG = 60, targetColourAB = 90; //(255,236,245) red (255,205,220)
  int targetColourBR = 115, targetColourBG = 120, targetColourBB = 90; //(232,238,251) blue (210,212,232)
  int targetColourCR = 90, targetColourCG = 90, targetColourCB = 100; //(248,255,238) green (232,240,213)
  // The threshold value determines how similar the colours have to be to match - higher values = less sensitive
  int threshold = 50;
  // Call the getColourDistance function to determine the distance between the colours, and compare to the threshold
  if(getColourDistance((int)colour[0], (int)colour[1], (int)colour[2], targetColourAR, targetColourAG, targetColourAB) < threshold) {
    Serial.println(F("The sensor has detected the target colour!"));
    displayValue = "17";

  } else if (getColourDistance((int)colour[0], (int)colour[1], (int)colour[2], targetColourBR, targetColourBG, targetColourBB) < threshold) {
    Serial.println(F("The sensor has detected the target colour!"));
    displayValue = "34";
  
  } else if (getColourDistance((int)colour[0], (int)colour[1], (int)colour[2], targetColourCR, targetColourCG, targetColourCB) < threshold) {
    Serial.println(F("The sensor has detected the target colour!"));
    displayValue = "86";
  } else {
    displayValue = ("...");
  }
  // Set the LEDs to match the colour detected
 showDisplay();

}

// Test the similarity between two colours by calculating the Euclidean distance between their RGB values
// See https://en.wikipedia.org/wiki/Color_difference
int getColourDistance(int r1, int g1, int b1, int r2, int g2, int b2) {
  //return (pow(r1-r2, 2) + pow(g1-g2, 2) + pow(b1-b2, 2));
  return (abs(r1-r2) + abs(g1-g2) + abs(b1-b2));
}