/*
 * LabUI Arduino Firmware v1.1
 * ─────────────────────────────────────────────────────────────────────────
 * Upload this sketch to your Arduino.
 * Set baud rate to 115200 in both this sketch and LabUI.
 *
 * WHAT IT DOES
 *   Continuously streams analog + digital readings to the PC.
 *   Listens for commands from the PC and executes them.
 *
 * RECEIVED COMMANDS (PC → Arduino):
 *   W:D13:1          digitalWrite(13, HIGH)
 *   W:D6:0           digitalWrite(6,  LOW)
 *   P:D9:128         analogWrite(9, 128)   → ~50% PWM
 *   C:SETPOINT:75.0  store named parameter
 *   Q:A0             reply immediately with A0 reading
 *   R:ALL            send full data frame immediately
 *   X:STOP           set all outputs LOW
 *   X:RESET          reset to defaults
 *
 * SENT DATA (Arduino → PC):
 *   A0:3.142,A1:0.015,D2:0,D3:1\n     (every SEND_INTERVAL ms)
 *   ACK:W:D13:1\n                       (after each command)
 *   ERR:Unknown command\n               (on parse failure)
 * ─────────────────────────────────────────────────────────────────────────
 */

// ── Configuration ─────────────────────────────────────────────────────────
int ANALOG_PINS[6]  = {A0, A1, A2, A3, A4, A5}; // actual pin numbers (reconfigured by APIN:)
int ANALOG_IDX[6]   = {0, 1, 2, 3, 4, 5};        // label indices used in output (A0, A1, …)
int DIGITAL_IN[16]  = {2, 3, 4};   // reconfigured at runtime via DPIN:IN:
int DIGITAL_OUT[16] = {5, 6, 7, 9, 10, 11};   // reconfigured via DPIN:OUT:
int N_ANALOG        = 0;    // set by APIN: command (0 = inactive until configured by PC)
int N_DIG_IN        = 0;    // set by DPIN:IN: command (0 = inactive until configured)
int N_DIG_OUT       = 0;    // set by DPIN:OUT: command (0 = inactive until configured)
const int SEND_INTERVAL  = 50;   // ms between data frames (50 = 20 Hz)
const long BAUD_RATE     = 115200;

// ── Named parameters (set via C: commands) ──────────────────────────────
float param_setpoint = 0.0;
float param_kp       = 1.0;
float param_ki       = 0.0;
float param_kd       = 0.0;
int   param_mode     = 0;

unsigned long lastSend = 0;

// ── Setup ─────────────────────────────────────────────────────────────────
void setup() {
  Serial.begin(BAUD_RATE);
  for (int i = 0; i < N_DIG_IN;  i++) pinMode(DIGITAL_IN[i],  INPUT_PULLUP);
  for (int i = 0; i < N_DIG_OUT; i++) pinMode(DIGITAL_OUT[i], OUTPUT);
}

// ── Main loop ─────────────────────────────────────────────────────────────
void loop() {
  handleCommands();
  sendData();
}

// ── Command handler ───────────────────────────────────────────────────────
void handleCommands() {
  if (!Serial.available()) return;

  String cmd = Serial.readStringUntil('\n');
  cmd.trim();
  if (cmd.length() == 0) return;

  char type = cmd.charAt(0);

  // W:Dxx:val  — digital write
  if (type == 'W') {
    int c1 = cmd.indexOf(':', 2);
    if (c1 < 0) { Serial.println("ERR:Bad W format"); return; }
    String pinStr = cmd.substring(2, c1);
    int    val    = cmd.substring(c1 + 1).toInt();
    int    pin    = pinStr.substring(1).toInt();  // strip 'D'
    digitalWrite(pin, val ? HIGH : LOW);
    Serial.print("ACK:"); Serial.println(cmd);
  }

  // P:Dxx:duty  — PWM write (0-255)
  else if (type == 'P') {
    int c1 = cmd.indexOf(':', 2);
    if (c1 < 0) { Serial.println("ERR:Bad P format"); return; }
    int pin  = cmd.substring(2, c1).substring(1).toInt();
    int duty = constrain(cmd.substring(c1 + 1).toInt(), 0, 255);
    analogWrite(pin, duty);
    Serial.print("ACK:"); Serial.println(cmd);
  }

  // C:NAME:value  — set named parameter
  else if (type == 'C') {
    int c1 = cmd.indexOf(':', 2);
    int c2 = cmd.indexOf(':', c1 + 1);
    if (c1 < 0 || c2 < 0) { Serial.println("ERR:Bad C format"); return; }
    String name = cmd.substring(2, c1);
    float  val  = cmd.substring(c2 + 1).toFloat();
    if      (name == "SETPOINT") param_setpoint = val;
    else if (name == "KP")       param_kp       = val;
    else if (name == "KI")       param_ki       = val;
    else if (name == "KD")       param_kd       = val;
    else if (name == "MODE")     param_mode     = (int)val;
    // Add your own parameters here:
    // else if (name == "SPEED") motor_speed = val;
    Serial.print("ACK:"); Serial.println(cmd);
  }

  // Q:NAME  — immediate query
  else if (type == 'Q') {
    String name = cmd.substring(2);
    if      (name == "SETPOINT") { Serial.print("SETPOINT:"); Serial.println(param_setpoint, 3); }
    else if (name == "KP")       { Serial.print("KP:");       Serial.println(param_kp, 4); }
    else {
      // Try to read as analog pin  Q:A0
      if (name.charAt(0) == 'A') {
        int pin = name.substring(1).toInt();
        float v = analogRead(pin) * (5.0 / 1023.0);
        Serial.print(name); Serial.print(":"); Serial.println(v, 3);
      }
    }
  }

  // R:ALL  — send full frame immediately
  else if (type == 'R') {
    sendFrame();
  }

  // X:STOP / X:RESET  — emergency stop
  else if (type == 'X') {
    String sub = cmd.substring(2);
    for (int i = 0; i < N_DIG_OUT; i++) digitalWrite(DIGITAL_OUT[i], LOW);
    Serial.print("ACK:"); Serial.println(cmd);
  }

  // APIN:0,1,3  — reconfigure analog inputs at runtime (indices 0–5 into A0–A5)
  else if (type == 'A') {
    if (!cmd.startsWith("APIN:")) { Serial.println("ERR:Bad A command"); return; }
    String pinList = cmd.substring(5);   // "0,1,3"
    const int _APINS[] = {A0,A1,A2,A3,A4,A5};
    N_ANALOG = 0;
    int start = 0;
    for (int i = 0; i <= (int)pinList.length(); i++) {
      if (i == (int)pinList.length() || pinList[i] == ',') {
        String tok = pinList.substring(start, i); tok.trim();
        if (tok.length() > 0 && N_ANALOG < 6) {
          int idx = tok.toInt();
          if (idx >= 0 && idx < 6) {
            ANALOG_PINS[N_ANALOG] = _APINS[idx];
            ANALOG_IDX[N_ANALOG]  = idx;
            N_ANALOG++;
          }
        }
        start = i + 1;
      }
    }
    Serial.print("ACK:"); Serial.println(cmd);
  }

  // DPIN:IN:2,3,8  or  DPIN:OUT:5,6,7  — reconfigure digital I/O pins at runtime
  else if (type == 'D') {
    int c1 = cmd.indexOf(':', 2);
    int c2 = (c1 >= 0) ? cmd.indexOf(':', c1 + 1) : -1;
    if (c1 < 0 || c2 < 0) { Serial.println("ERR:Bad DPIN format"); return; }
    String dir     = cmd.substring(2, c1);    // "IN" or "OUT"
    String pinList = cmd.substring(c2 + 1);   // "2,3,8"
    bool isIn = (dir == "IN");
    int* arr  = isIn ? DIGITAL_IN  : DIGITAL_OUT;
    int* cnt  = isIn ? &N_DIG_IN   : &N_DIG_OUT;
    int  mode = isIn ? INPUT_PULLUP : OUTPUT;
    *cnt = 0;
    int start = 0;
    for (int i = 0; i <= (int)pinList.length(); i++) {
      if (i == (int)pinList.length() || pinList[i] == ',') {
        String tok = pinList.substring(start, i); tok.trim();
        if (tok.length() > 0 && *cnt < 16) {
          arr[*cnt] = tok.toInt();
          pinMode(arr[*cnt], mode);
          (*cnt)++;
        }
        start = i + 1;
      }
    }
    Serial.print("ACK:"); Serial.println(cmd);
  }

  else {
    Serial.print("ERR:Unknown command: "); Serial.println(cmd);
  }
}

// ── Data sender ───────────────────────────────────────────────────────────
void sendData() {
  if (millis() - lastSend < SEND_INTERVAL) return;
  lastSend = millis();
  sendFrame();
}

void sendFrame() {
  String out = "";
  bool first = true;
  // Analog inputs — converted to 0.0–5.0 V
  for (int i = 0; i < N_ANALOG; i++) {
    float v = analogRead(ANALOG_PINS[i]) * (5.0 / 1023.0);
    if (!first) out += ",";
    out += "A" + String(ANALOG_IDX[i]) + ":" + String(v, 3);
    first = false;
  }
  // Digital inputs (INPUT_PULLUP — invert so pressed=1)
  // These share the same output line as analog — one line per frame.
  for (int i = 0; i < N_DIG_IN; i++) {
    if (!first) out += ",";
    out += "D" + String(DIGITAL_IN[i]) + ":" + String(!digitalRead(DIGITAL_IN[i]));
    first = false;
  }
  Serial.println(out);
  // Example combined output: A0:3.142,D2:0,D3:1
}