// Arduino Sketch: RGB to CMYK Conversion + Timer Mapping + Motor & Water Control
//
// Functionality:
// 1. Read serial commands:
//    - "R,G,B" : Convert to CMYK, map to durations, and output results
//    - "W1","W2","W3": Trigger water motor timings
//    - "M": Run motor test sequence
// 2. CMYK mapping: 0–100% → 0–maxDelay (default 1000 ms)
// 3. Motor control: 6 motors controlled via pin pairs

// ----- User-Adjustable Settings -----
const unsigned int maxDelayC = 1000;  // Maximum mapped delay for Cyan 100% (in milliseconds)
const unsigned int maxDelayY = 1000;  // Maximum mapped delay for Magenta 100% (in milliseconds)
const unsigned int maxDelayM = 1000;  // Maximum mapped delay for Yellow 100% (in milliseconds)
const unsigned int maxDelayK = 1000;  // Maximum mapped delay for Black 100% (in milliseconds)

// Motor pins: each motor uses two pins for forward/backward control
// Storing motor pins inside 2d array
const int motorPins[5][2] = {
  { 37, 38 },  // Motor 1 (C)
  { 47, 48 },  // Motor 2 (M)
  { 12, 13 },  // Motor 3 (Y)
  { 14, 21 },  // Motor 4 (K)
  { 9, 10 }    // Motor 5 (W)
};

// Water Time values for 3 different modes (in milliseconds)
int waterPourTime1 = 500;   // Time value to keep water motor running in milliseconds
int waterPourTime2 = 1000;  // Time value to keep water motor running in milliseconds
int waterPourTime3 = 1500;  // Time value to keep water motor running in milliseconds

void setup() {
  Serial.begin(9600);
  Serial.println("Ready. Enter RGB (e.g. 255,100,0), W1/W2/W3 for water, or M for motors.");

  // Initialize motor pins
  for (int i = 0; i < 5; i++) {
    pinMode(motorPins[i][0], OUTPUT);
    pinMode(motorPins[i][1], OUTPUT);
    digitalWrite(motorPins[i][0], LOW);
    digitalWrite(motorPins[i][1], LOW);
  }
}

void loop() {
  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();

    // Water commands
    if (cmd == "W1") {
      triggerWater(waterPourTime1);  // Run water motor for defined time
    } else if (cmd == "W2") {
      triggerWater(waterPourTime2);  // Run water motor for defined time
    } else if (cmd == "W3") {
      triggerWater(waterPourTime3);  // Run water motor for defined time
      // Motor test command
    } else if (cmd == "M") {
      runMotorTest();
      // RGB to CMYK
    } else if (isRGB(cmd)) {
      int r, g, b;
      sscanf(cmd.c_str(), "%d,%d,%d", &r, &g, &b);
      float c, m, y, k;
      convertRGBtoCMYK(r, g, b, c, m, y, k);

      // Map CMYK percentages to delays
      unsigned int dC = mapPercentToDelay(c, maxDelayC);
      unsigned int dM = mapPercentToDelay(m, maxDelayM);
      unsigned int dY = mapPercentToDelay(y, maxDelayY);
      unsigned int dK = mapPercentToDelay(k, maxDelayK);

      // Output results
      Serial.print("CMYK (%) → C:");
      Serial.print(c, 1);
      Serial.print(" (Delay ");
      Serial.print(dC);
      if (dC > 0) { // Check if the value is greater then 0
        runCMYKMotor(0, dC);  // Run C motor for dc time
      }
      Serial.println(" ms)");
      Serial.print("           M:");
      Serial.print(m, 1);
      Serial.print(" (Delay ");
      Serial.print(dM);
      Serial.println(" ms)");
      if (dM > 0) { // Check if the value is greater then 0
        runCMYKMotor(1, dM);  // Run M motor for dM time
      }
      Serial.print("           Y:");
      Serial.print(y, 1);
      Serial.print(" (Delay ");
      Serial.print(dY);
      Serial.println(" ms)");
      if (dY > 0) { // Check if the value is greater then 0
        runCMYKMotor(2, dY);  // Run Y motor for dY time
      }
      Serial.print("           K:");
      Serial.print(k, 1);
      Serial.print(" (Delay ");
      Serial.print(dK);
      Serial.println(" ms)");
      if (dK > 0) { // Check if the value is greater then 0
        runCMYKMotor(3, dK);  // Run K motor for dK time
      }

    } else {
      Serial.println("Unknown command. Use R,G,B or W1/W2/W3 or M");
    }
  }
}

// Check if input string matches "num,num,num"
bool isRGB(const String &s) {
  int parts = 0;
  for (unsigned int i = 0; i < s.length(); i++)
    if (s.charAt(i) == ',') parts++;
  return (parts == 2);
}

// Convert RGB (0-255) to CMYK (0.0-100.0)
void convertRGBtoCMYK(int r, int g, int b, float &c, float &m, float &y, float &k) {
  float rf = r / 255.0;
  float gf = g / 255.0;
  float bf = b / 255.0;

  k = 1.0 - max(max(rf, gf), bf);
  if (k >= 1.0) {
    c = m = y = 0.0;
  } else {
    c = (1.0 - rf - k) / (1.0 - k);
    m = (1.0 - gf - k) / (1.0 - k);
    y = (1.0 - bf - k) / (1.0 - k);
  }

  // Convert to percentage
  c *= 100.0;
  m *= 100.0;
  y *= 100.0;
  k *= 100.0;
}

// Map percentage (0.0-100.0) to delay (0-maxDelay)
unsigned int mapPercentToDelay(float pct,  int maxDelay) {
  return (unsigned int)((pct / 100.0) * maxDelay);
}

// Trigger water motor for given duration (ms)
void triggerWater(unsigned int duration) {
  Serial.print("Water ON for ");
  Serial.print(duration);
  Serial.println(" ms");
  digitalWrite(motorPins[4][0], HIGH); // Start water motor
  digitalWrite(motorPins[4][1], LOW);
  delay(duration);
  digitalWrite(motorPins[4][0], LOW); // stop water motor
  digitalWrite(motorPins[4][1], LOW);
}
void runCMYKMotor(int index, int delayValue) {
  // Get index of CMYK motor that needs to be run
  // Get delay value for how long the motor needs to run
  // Start Motor
  digitalWrite(motorPins[index][0], HIGH);
  digitalWrite(motorPins[index][1], LOW);
  delay(delayValue); // wait for defined time
  // Stop running motor
  digitalWrite(motorPins[index][0], LOW);
  digitalWrite(motorPins[index][1], LOW);

}
// Sequentially run each motor forward then backward
void runMotorTest() {
  Serial.println("Starting motor test...");
  for (int i = 0; i < 5; i++) {
    Serial.print("Run Motor ");
    Serial.print(i);
    Serial.println(" forward for 5 secodns");
    digitalWrite(motorPins[i][0], HIGH);
    digitalWrite(motorPins[i][1], LOW);
    delay(5000);

    // Turn off
    digitalWrite(motorPins[i][0], LOW);
    digitalWrite(motorPins[i][1], LOW);
  }
  Serial.println("Motor test complete.");
}