int sensors[3] = {2, 2, 2};
String directions[3] = {"Turn left", "Go forward", "Turn right"};
int lastDirection = 1;

enum State { SCANNING, DECIDING, MOVING, STUCK };
State currentState = SCANNING;

int findMax(int arr[], int size) {
    int maxIndex = 0;
    for (int i = 1; i < size; i++) {
        if (arr[i] > arr[maxIndex]) maxIndex = i;
    }
    return maxIndex;
}

int breakTie(int arr[], int size, int winner) {
    int count = 0;
    for (int i = 0; i < size; i++) {
        if (arr[i] == arr[winner]) count++;
    }
    return count;
}

void setup() {
    Serial.begin(9600);
}

void loop() {
    switch (currentState) {

        case SCANNING:
            Serial.println("SCANNING");
            currentState = DECIDING;
            break;

        case DECIDING: {
            Serial.println("DECIDING");
            int winner = findMax(sensors, 3);
            int count_winner = breakTie(sensors, 3, winner);
            if (count_winner > 1) {
                for (int i = 0; i < 3; i++) {
                    if (sensors[i] == sensors[winner] && i == lastDirection) {
                        winner = i;
                    }
                }
            }
            lastDirection = winner;
            Serial.println(directions[winner]);
            if (sensors[0] < 5 && sensors[1] < 5 && sensors[2] < 5) {
                currentState = STUCK;
            } else {
                currentState = MOVING;
            }
            break;
        }

        case MOVING:
            Serial.println("MOVING");
            // Otto movement code goes here
            currentState = SCANNING;
            break;

        case STUCK:
            Serial.println("STUCK");
            // Otto stop code goes here
            break;
    }
}