int red = 28;
int green = 27;
int blue = 26;
int led = 2;
int button = 3;
int count = 0;

bool currentBS; // button state
bool previousBS = HIGH; // for edge detection
bool blueOn = false;
bool redOn = false;

char motor = 'A';

unsigned long pressStartTime = 0;
bool isLongPressHandled = false;

enum ShortPressState {OFF, LEFT, RIGHT};
ShortPressState shortPressState = OFF;

//#define CMD_SWITCH_MOTOR 0x10
//#define CMD_CYCLE_STATE  0x20

void setup() {
  Serial1.begin(57600);  // Start UART on Serial1 at 57600 baud
  pinMode(button, INPUT_PULLUP);
  pinMode(red, OUTPUT);
  pinMode(green, OUTPUT);
  pinMode(blue, OUTPUT);
  pinMode(led, OUTPUT);  
  turnOffAllLEDs();
}

void loop() {
  currentBS = digitalRead(button); // 0=pressed, 1=not pressed
  // Detect button press (falling edge)
  if (previousBS == HIGH && currentBS == LOW) {
    pressStartTime = millis();
    isLongPressHandled = false;
  }
  // Detect button release (rising edge)
  if (previousBS == LOW && currentBS == HIGH) {
    unsigned long pressDuration = millis() - pressStartTime;
    if (!isLongPressHandled) {
      if (pressDuration < 500) {
        handleShortPress();
      }
    }
  }
  // Long press detection while button held
  if (currentBS == LOW && !isLongPressHandled) {
    unsigned long pressDuration = millis() - pressStartTime;
    if (pressDuration >= 1000) {
      handleLongPress();
      if (motor=='A'){
        turnOnBlue();
        motor='B';    
      }
      else{
        turnOnRed();
        motor='A';
      }
      isLongPressHandled = true;
    }
  }
  previousBS = currentBS;
  delay(10);
}

void handleShortPress() {
  Serial1.print(motor);
  Serial.print(motor);
  if (shortPressState == OFF) {
    digitalWrite(green, LOW);
    shortPressState = RIGHT;
  }
  else if (shortPressState == RIGHT) {
    Serial1.print('R');
    Serial.print('R');
    shortPressState = LEFT;
  }
  else { 
    Serial1.print('L');
    Serial.print('L');
    shortPressState = OFF;
  }
}

void handleLongPress() {
  switcheroo();
  digitalWrite(led, LOW);
  shortPressState = OFF;
}

void switcheroo() {
  if (redOn) {
    turnOnBlue();
  } else if (blueOn) {
    turnOnRed();
  }
}

void turnOffAllLEDs() {
  digitalWrite(red, HIGH);
  digitalWrite(green, HIGH);
  digitalWrite(blue, HIGH);
}

void turnOnBlue() {
  redOn = false;
  turnOffAllLEDs();
  digitalWrite(blue, LOW);
  blueOn = true;
}

void turnOnRed() {
  blueOn = false;
  turnOffAllLEDs();
  digitalWrite(red, LOW);
  redOn = true;
}