#include <Wire.h>
#include "Adafruit_TCS34725.h"
#include <Servo.h>
#include <AccelStepper.h>
AccelStepper RotationMotor(8, 8, 10, 9, 11);  // HALFSTEP mode, take care!! output is 1,3,2,4
Servo SortingServo;

Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_101MS, TCS34725_GAIN_4X);


#define MININTENSITY 120
#define MEASINTERVAL 110  // ms


//  R , G  , B
const unsigned int Colors[][4] = {
  { 125, 65, 65, 550 },    // red
  { 120, 60, 75, 800 },    // pink
  { 135, 60, 60, 800 },  // orange
  { 96, 98, 60, 950 },    // yellow
  { 73, 103, 77, 700 },    // green
  { 65, 93, 94, 600 },     // blue 
  { 85, 75, 96, 550 },     // purple
  { 83, 90, 80, 1000 },     // white
};



const String ColorNames[] = {
  "Red",
  "Pink",
  "Orange",
  "Yellow",
  "Green",
  "Blue",
  "Purple",
  "White"
};


// starts at 0
const int NumOfColors = sizeof(Colors) / sizeof(Colors[0]);
uint16_t BestMatch[6]; // r,g,b,intensity,color,fitresult
uint32_t LastMeasurementTime;
uint16_t red, green, blue, intensity;
uint8_t red_norm, green_norm, blue_norm;
  
enum SorterState {
  WAITING_FOR_NEXT_BEAD,
  MEASURING,
  SORTING
};
enum SorterState Sorter_State;


// We don't need to wait for the next integration cycle because we receive an interrupt when the integration cycle is complete
void GetColorMeasurement() {
  intensity = tcs.read16(TCS34725_CDATAL);
  red = tcs.read16(TCS34725_RDATAL);
  green = tcs.read16(TCS34725_GDATAL);
  blue = tcs.read16(TCS34725_BDATAL);

/*
  // Nomalize that total is 255
  intensity = red + green + blue;
  red_norm = (float)red / intensity * 255.0;
  green_norm = (float)green / intensity * 255.0;
  blue_norm = (float)blue / intensity * 255.0;
*/

// Poor mans Hue calculator..
uint16_t IntensityMin, IntensityMax;
// Remove the "white" component in the RGB values
IntensityMin = min(red,green);
IntensityMin = min(IntensityMin, blue);
red_norm = red - IntensityMin;
green_norm = green - IntensityMin;
blue_norm = blue - IntensityMin;
// Normalize
IntensityMax = max(red_norm,green_norm);
IntensityMax = max(IntensityMax, blue_norm);
red_norm = (float)red_norm / IntensityMax * 255.0;
green_norm = (float)green_norm / IntensityMax * 255.0;
blue_norm = (float)blue_norm / IntensityMax * 255.0;


	Serial.print(red, DEC); Serial.print(", "); 
	Serial.print(green, DEC); Serial.print(", "); 
	Serial.print(blue, DEC); Serial.print(", "); 
	Serial.print(intensity, DEC); Serial.print(", "); 

	Serial.print(red_norm, DEC); Serial.print(", "); 
	Serial.print(green_norm, DEC); Serial.print(", "); 
	Serial.print(blue_norm, DEC); Serial.print(", "); 
	Serial.println(intensity, DEC); 

}

//This function compares colors and updates BestMatch values
void MatchColor(){
	unsigned int MatchResult[NumOfColors];
	uint8_t MatchedColor = 0;
	//Serial.print(" Compare: ");
	for (int i = 0; i < NumOfColors; i++) {
	  MatchResult[i] = sq(red_norm - Colors[i][0]) + sq(green_norm - Colors[i][1]) + sq(blue_norm - Colors[i][2]);
	  //MatchResult[i] = abs(intensity - Colors[i][3]);
	  //Serial.print(MatchResult[i]); Serial.print(" ");
	  if (MatchResult[i] < MatchResult[MatchedColor]) {
		MatchedColor = i;
	  }
	}
	
	if (intensity > BestMatch[3]) {
	  BestMatch[0] = red_norm;
	  BestMatch[1] = green_norm;
	  BestMatch[2] = blue_norm;
	  BestMatch[3] = intensity;
	  BestMatch[4] = MatchedColor;
	  BestMatch[5] = MatchResult[MatchedColor];
	}
	
	Serial.print(red_norm, DEC); Serial.print(", ");
	Serial.print(green_norm, DEC); Serial.print(", ");
	Serial.print(blue_norm, DEC); Serial.print(", ");
	Serial.print(intensity, DEC); Serial.print(", ");
	Serial.print(ColorNames[MatchedColor]); Serial.print(", ");
	Serial.println(MatchResult[MatchedColor]);
}

// Check serial port data and handle input
void ReadSerialPort() {
	  // TEMP: Use to adjust rotation speed
  if (Serial.available()) {
    int speed = Serial.parseInt();
    if ((speed > 10) && (speed < 1000)) {
      RotationMotor.setSpeed(speed);
    } else {
      Serial.println("Incorrect input");
    }
  }
}

void setup() {
  // Initialize Serial
  Serial.begin(115200);

  // Initialize Color sensor
  if (tcs.begin()) {
    Serial.println("Found sensor");
  } else {
    Serial.println("No TCS34725 found ... check your connections");
    //while (1);  // halt!
  }

  // Initialize Sorting servo
  SortingServo.attach(13);
  SortingServo.write(90);

  // Initialize Rotation motor
  // 2038 steps per rev, 339.66 steps between beads
  // every 2 seconds 1 bead --> ~170 steps/s
  // every 3 seconds 1 bead --> ~113 steps/s
  // every 4 seconds 1 bead --> ~85 steps/s
  RotationMotor.setMaxSpeed(1000);
  RotationMotor.setSpeed(500);

  Sorter_State = WAITING_FOR_NEXT_BEAD;
  LastMeasurementTime = millis();
}



void loop() {
  // Update stepper motor
  RotationMotor.runSpeed();

  ReadSerialPort();

  // Check if measurement timer passed
  if (millis() > LastMeasurementTime + MEASINTERVAL) {
    LastMeasurementTime = millis();

    // Get measurement
    GetColorMeasurement();

    switch (Sorter_State) {
      case SORTING:
        // Set servo position
        SortingServo.write(19 + BestMatch[4] * 22.5);

        Serial.print("Best match: ");
        Serial.print(ColorNames[BestMatch[4]]); Serial.print(" ");
        Serial.print(BestMatch[0], DEC); Serial.print(", ");
        Serial.print(BestMatch[1], DEC); Serial.print(", ");
        Serial.print(BestMatch[2], DEC); Serial.print(", ");
        Serial.print(BestMatch[3], DEC); Serial.print(", ");
        Serial.println(BestMatch[5], DEC);
		
        Sorter_State = WAITING_FOR_NEXT_BEAD;

        break;

      case WAITING_FOR_NEXT_BEAD:

        if (intensity < MININTENSITY) {
          break;
        }
		
		// Do not break at first correct measurement, otherwise this measurement is omitted
        BestMatch[3] = 0;  // clear intensity
        Sorter_State = MEASURING;

      case MEASURING:

        if (intensity < MININTENSITY) {
          Sorter_State = SORTING;
          break;
        }

		MatchColor();

        break;
    }
  }
}