#define PIN_S0   3
#define PIN_S1   4
#define PIN_S2   5
#define PIN_S3   6
#define PIN_OUT  7

// Calibration
// WHITE: intensity=255, 20% scale → freq=100 kHz → half-period = 5 µs
// BLACK: intensity=0 → freq=0 → pulseIn returns 0 → handled separately
// TIMEOUT: anything slower than ~2 Hz (half-period > 250,000 µs) is treated as black
#define CAL_WHITE_US   5UL
#define PULSE_TIMEOUT  50000UL

// Samples per channel
#define SAMPLES  5

// readChannel — select filter, wait, average SAMPLES pulses
// Returns average half-period in µs, or 0 if black/timeout
unsigned long readChannel(bool s2, bool s3)
{
    digitalWrite(PIN_S2, s2);
    digitalWrite(PIN_S3, s3);
    delay(10);

    unsigned long total = 0;
    uint8_t valid = 0;

    for (uint8_t i = 0; i < SAMPLES; i++)
    {
        unsigned long pw = pulseIn(PIN_OUT, HIGH, PULSE_TIMEOUT);
        if (pw > 0)
        {
            total += pw;
            valid++;
        }
    }

    return (valid > 0) ? (total / valid) : 0;
}

// toValue — convert half-period (µs) to 0–255
int toValue(unsigned long pulseUs)
{
    if (pulseUs == 0) return 0;
    if (pulseUs <= CAL_WHITE_US) return 255;

    unsigned long freq = 500000UL / pulseUs;
    int val = (int)((unsigned long)freq * 255UL / 100000UL);

    if (val < 0) val = 0;
    if (val > 255) val = 255;

    return val;
}

void setup()
{
    pinMode(PIN_S0, OUTPUT);
    pinMode(PIN_S1, OUTPUT);
    pinMode(PIN_S2, OUTPUT);
    pinMode(PIN_S3, OUTPUT);
    pinMode(PIN_OUT, INPUT);

    digitalWrite(PIN_S0, HIGH);
    digitalWrite(PIN_S1, LOW);

    Serial.begin(9600);
    Serial.println("TCS3200 ready.");
    Serial.println();
}

void loop()
{
    // S2=L,S3=L → Red
    // S2=H,S3=H → Green
    // S2=L,S3=H → Blue
    unsigned long rawR = readChannel(LOW, LOW);
    unsigned long rawG = readChannel(HIGH, HIGH);
    unsigned long rawB = readChannel(LOW, HIGH);

    int r = toValue(rawR);
    int g = toValue(rawG);
    int b = toValue(rawB);

    Serial.print("R: "); Serial.print(r);
    Serial.print(" G: "); Serial.print(g);
    Serial.print(" B: "); Serial.print(b);
    Serial.println();

    delay(500);
}