#define LEFT_DISTANCE_SENSOR_TRIG_PIN 32
#define LEFT_DISTANCE_SENSOR_ECHO_PIN 33

#define RIGHT_DISTANCE_SENSOR_TRIG_PIN 25
#define RIGHT_DISTANCE_SENSOR_ECHO_PIN 26

#define L298N_LEFT_FORWARD_PIN 13
#define L298N_LEFT_BACKWARD_PIN 14
#define L298N_LEFT_SPEED_PIN 27

#define L298N_RIGHT_FORWARD_PIN 17
#define L298N_RIGHT_BACKWARD_PIN 2
#define L298N_RIGHT_SPEED_PIN 5

#define LIGHT_SENSOR_PIN 34
#define LED_PIN 4

// Config
#define SPEED_OF_SOUND 0.017                       // 0.034 cm/us / 2
#define MOTOR_SPEED 128                            // 50% PWM
#define MAX_TIMEOUT_FOR_OBSTACLE_DETECTION_US 5000 // 5ms = 85cm max
#define MAX_DISTANCE_FOR_OBSTACLE_DETECTION_CM 10
#define LIGHT_THRESHOLD 550

#define TASK_DELAY_MS 100

#define QUEUE_SIZE 20
#define TASK_STACK_SIZE 2048

#define STATE_COUNT 14
#define EVENT_COUNT 10

#include <Arduino.h>

#define STATE_COUNT 14
#define EVENT_COUNT 10

enum class State : uint8_t {
  // Light off mode
  STILL = 0,
  FORWARD = 1,
  BACKWARD = 2,
  LEFT = 3,
  RIGHT = 4,
  AVOIDING_OBSTACLE_LEFT = 5,
  AVOIDING_OBSTACLE_RIGHT = 6,

  // Light on mode
  STILL_LIGHT_ON = 7,
  FORWARD_LIGHT_ON = 8,
  BACKWARD_LIGHT_ON = 9,
  LEFT_LIGHT_ON = 10,
  RIGHT_LIGHT_ON = 11,
  AVOIDING_OBSTACLE_LEFT_LIGHT_ON = 12,
  AVOIDING_OBSTACLE_RIGHT_LIGHT_ON = 13,
};

enum class Event : uint8_t {
  OBSTACLE_LEFT = 0,
  OBSTACLE_RIGHT = 1,
  PATH_FREE = 2,
  MOVE_FORWARD = 3,
  MOVE_LEFT = 4,
  MOVE_RIGHT = 5,
  MOVE_BACKWARD = 6,
  STOP = 7,
  LOW_LIGHT = 8,
  HIGH_LIGHT = 9
};

using SensorFunction = Event (*)();
using Action = void (*)();

struct Transition {
  Action action;
  State nextState;
};

// Handlers
void switchOnLED() {
  digitalWrite(LED_PIN, HIGH);
}

void switchOffLED() {
  digitalWrite(LED_PIN, LOW);
}

void stopMotors() {
  digitalWrite(L298N_LEFT_FORWARD_PIN, LOW);
  digitalWrite(L298N_LEFT_BACKWARD_PIN, LOW);
  digitalWrite(L298N_RIGHT_FORWARD_PIN, LOW);
  digitalWrite(L298N_RIGHT_BACKWARD_PIN, LOW);
}

void moveForward() {
  stopMotors();
  digitalWrite(L298N_LEFT_FORWARD_PIN, HIGH);
  digitalWrite(L298N_RIGHT_FORWARD_PIN, HIGH);
}

void moveBackward() {
  stopMotors();
  digitalWrite(L298N_LEFT_BACKWARD_PIN, HIGH);
  digitalWrite(L298N_RIGHT_BACKWARD_PIN, HIGH);
}

void moveLeft() {
  stopMotors();
  digitalWrite(L298N_LEFT_BACKWARD_PIN, HIGH);
  digitalWrite(L298N_RIGHT_FORWARD_PIN, HIGH);
}

void moveRight() {
  stopMotors();
  digitalWrite(L298N_LEFT_FORWARD_PIN, HIGH);
  digitalWrite(L298N_RIGHT_BACKWARD_PIN, HIGH);
}

const Transition sm[STATE_COUNT][EVENT_COUNT] = {
  /* STILL */
  { /* OBSTACLE_LEFT */ { NULL, State::STILL },
    /* OBSTACLE_RIGHT */ { NULL, State::STILL },
    /* PATH_FREE */ { NULL, State::STILL },
    /* MOVE_FORWARD */ { moveForward, State::FORWARD },
    /* MOVE_LEFT */ { moveLeft, State::LEFT },
    /* MOVE_RIGHT */ { moveRight, State::RIGHT },
    /* MOVE_BACKWARD */ { moveBackward, State::BACKWARD },
    /* STOP */ { NULL, State::STILL },
    /* LOW_LIGHT */ { switchOnLED, State::STILL_LIGHT_ON },
    /* HIGH_LIGHT */ { NULL, State::STILL } },
  /* FORWARD */
  { /* OBSTACLE_LEFT */ { moveRight, State::AVOIDING_OBSTACLE_LEFT },
    /* OBSTACLE_RIGHT */ { moveLeft, State::AVOIDING_OBSTACLE_RIGHT },
    /* PATH_FREE */ { NULL, State::FORWARD },
    /* MOVE_FORWARD */ { moveForward, State::FORWARD },
    /* MOVE_LEFT */ { moveLeft, State::LEFT },
    /* MOVE_RIGHT */ { moveRight, State::RIGHT },
    /* MOVE_BACKWARD */ { moveBackward, State::BACKWARD },
    /* STOP */ { stopMotors, State::STILL },
    /* LOW_LIGHT */ { switchOnLED, State::FORWARD_LIGHT_ON },
    /* HIGH_LIGHT */ { NULL, State::FORWARD } },
  /* BACKWARD */
  { /* OBSTACLE_LEFT */ { NULL, State::BACKWARD },
    /* OBSTACLE_RIGHT */ { NULL, State::BACKWARD },
    /* PATH_FREE */ { NULL, State::BACKWARD },
    /* MOVE_FORWARD */ { moveForward, State::FORWARD },
    /* MOVE_LEFT */ { moveLeft, State::LEFT },
    /* MOVE_RIGHT */ { moveRight, State::RIGHT },
    /* MOVE_BACKWARD */ { moveBackward, State::BACKWARD },
    /* STOP */ { stopMotors, State::STILL },
    /* LOW_LIGHT */ { switchOnLED, State::BACKWARD_LIGHT_ON },
    /* HIGH_LIGHT */ { NULL, State::BACKWARD } },
  /* LEFT */
  { /* OBSTACLE_LEFT */ { NULL, State::LEFT },
    /* OBSTACLE_RIGHT */ { NULL, State::LEFT },
    /* PATH_FREE */ { NULL, State::LEFT },
    /* MOVE_FORWARD */ { moveForward, State::FORWARD },
    /* MOVE_LEFT */ { moveLeft, State::LEFT },
    /* MOVE_RIGHT */ { moveRight, State::RIGHT },
    /* MOVE_BACKWARD */ { moveBackward, State::BACKWARD },
    /* STOP */ { stopMotors, State::STILL },
    /* LOW_LIGHT */ { switchOnLED, State::LEFT_LIGHT_ON },
    /* HIGH_LIGHT */ { NULL, State::LEFT } },
  /* RIGHT */
  { /* OBSTACLE_LEFT */ { NULL, State::RIGHT },
    /* OBSTACLE_RIGHT */ { NULL, State::RIGHT },
    /* PATH_FREE */ { NULL, State::RIGHT },
    /* MOVE_FORWARD */ { moveForward, State::FORWARD },
    /* MOVE_LEFT */ { moveLeft, State::LEFT },
    /* MOVE_RIGHT */ { moveRight, State::RIGHT },
    /* MOVE_BACKWARD */ { moveBackward, State::BACKWARD },
    /* STOP */ { stopMotors, State::STILL },
    /* LOW_LIGHT */ { switchOnLED, State::RIGHT_LIGHT_ON },
    /* HIGH_LIGHT */ { NULL, State::RIGHT } },
  /* AVOIDING_OBSTACLE_LEFT */
  { /* OBSTACLE_LEFT */ { NULL, State::AVOIDING_OBSTACLE_LEFT },
    /* OBSTACLE_RIGHT */ { NULL, State::AVOIDING_OBSTACLE_LEFT },
    /* PATH_FREE */ { moveForward, State::FORWARD },
    /* MOVE_FORWARD */ { NULL, State::AVOIDING_OBSTACLE_LEFT },
    /* MOVE_LEFT */ { NULL, State::AVOIDING_OBSTACLE_LEFT },
    /* MOVE_RIGHT */ { NULL, State::AVOIDING_OBSTACLE_LEFT },
    /* MOVE_BACKWARD */ { NULL, State::AVOIDING_OBSTACLE_LEFT },
    /* STOP */ { stopMotors, State::STILL },
    /* LOW_LIGHT */ { switchOnLED, State::AVOIDING_OBSTACLE_LEFT_LIGHT_ON },
    /* HIGH_LIGHT */ { NULL, State::AVOIDING_OBSTACLE_LEFT } },
  /* AVOIDING_OBSTACLE_RIGHT */
  { /* OBSTACLE_LEFT */ { NULL, State::AVOIDING_OBSTACLE_RIGHT },
    /* OBSTACLE_RIGHT */ { NULL, State::AVOIDING_OBSTACLE_RIGHT },
    /* PATH_FREE */ { moveForward, State::FORWARD },
    /* MOVE_FORWARD */ { NULL, State::AVOIDING_OBSTACLE_RIGHT },
    /* MOVE_LEFT */ { NULL, State::AVOIDING_OBSTACLE_RIGHT },
    /* MOVE_RIGHT */ { NULL, State::AVOIDING_OBSTACLE_RIGHT },
    /* MOVE_BACKWARD */ { NULL, State::AVOIDING_OBSTACLE_RIGHT },
    /* STOP */ { stopMotors, State::STILL },
    /* LOW_LIGHT */ { switchOnLED, State::AVOIDING_OBSTACLE_RIGHT_LIGHT_ON },
    /* HIGH_LIGHT */ { NULL, State::AVOIDING_OBSTACLE_RIGHT } },

  // Light on mode

  /* STILL_LIGHT_ON */
  { /* OBSTACLE_LEFT */ { NULL, State::STILL_LIGHT_ON },
    /* OBSTACLE_RIGHT */ { NULL, State::STILL_LIGHT_ON },
    /* PATH_FREE */ { NULL, State::STILL_LIGHT_ON },
    /* MOVE_FORWARD */ { moveForward, State::FORWARD_LIGHT_ON },
    /* MOVE_LEFT */ { moveLeft, State::LEFT_LIGHT_ON },
    /* MOVE_RIGHT */ { moveRight, State::RIGHT_LIGHT_ON },
    /* MOVE_BACKWARD */ { moveBackward, State::BACKWARD_LIGHT_ON },
    /* STOP */ { NULL, State::STILL_LIGHT_ON },
    /* LOW_LIGHT */ { NULL, State::STILL_LIGHT_ON },
    /* HIGH_LIGHT */ { switchOffLED, State::STILL } },
  /* FORWARD_LIGHT_ON */
  { /* OBSTACLE_LEFT */ { moveRight, State::AVOIDING_OBSTACLE_LEFT_LIGHT_ON },
    /* OBSTACLE_RIGHT */ { moveLeft, State::AVOIDING_OBSTACLE_RIGHT_LIGHT_ON },
    /* PATH_FREE */ { NULL, State::FORWARD_LIGHT_ON },
    /* MOVE_FORWARD */ { moveForward, State::FORWARD_LIGHT_ON },
    /* MOVE_LEFT */ { moveLeft, State::LEFT_LIGHT_ON },
    /* MOVE_RIGHT */ { moveRight, State::RIGHT_LIGHT_ON },
    /* MOVE_BACKWARD */ { moveBackward, State::BACKWARD_LIGHT_ON },
    /* STOP */ { stopMotors, State::STILL_LIGHT_ON },
    /* LOW_LIGHT */ { NULL, State::FORWARD_LIGHT_ON },
    /* HIGH_LIGHT */ { switchOffLED, State::FORWARD } },
  /* BACKWARD_LIGHT_ON */
  { /* OBSTACLE_LEFT */ { NULL, State::BACKWARD_LIGHT_ON },
    /* OBSTACLE_RIGHT */ { NULL, State::BACKWARD_LIGHT_ON },
    /* PATH_FREE */ { NULL, State::BACKWARD_LIGHT_ON },
    /* MOVE_FORWARD */ { moveForward, State::FORWARD_LIGHT_ON },
    /* MOVE_LEFT */ { moveLeft, State::LEFT_LIGHT_ON },
    /* MOVE_RIGHT */ { moveRight, State::RIGHT_LIGHT_ON },
    /* MOVE_BACKWARD */ { moveBackward, State::BACKWARD_LIGHT_ON },
    /* STOP */ { stopMotors, State::STILL_LIGHT_ON },
    /* LOW_LIGHT */ { NULL, State::BACKWARD_LIGHT_ON },
    /* HIGH_LIGHT */ { switchOffLED, State::BACKWARD } },
  /* LEFT_LIGHT_ON */
  { /* OBSTACLE_LEFT */ { NULL, State::LEFT_LIGHT_ON },
    /* OBSTACLE_RIGHT */ { NULL, State::LEFT_LIGHT_ON },
    /* PATH_FREE */ { NULL, State::LEFT_LIGHT_ON },
    /* MOVE_FORWARD */ { moveForward, State::FORWARD_LIGHT_ON },
    /* MOVE_LEFT */ { moveLeft, State::LEFT_LIGHT_ON },
    /* MOVE_RIGHT */ { moveRight, State::RIGHT_LIGHT_ON },
    /* MOVE_BACKWARD */ { moveBackward, State::BACKWARD_LIGHT_ON },
    /* STOP */ { stopMotors, State::STILL_LIGHT_ON },
    /* LOW_LIGHT */ { NULL, State::LEFT_LIGHT_ON },
    /* HIGH_LIGHT */ { switchOffLED, State::LEFT } },
  /* RIGHT_LIGHT_ON */
  { /* OBSTACLE_LEFT */ { NULL, State::RIGHT_LIGHT_ON },
    /* OBSTACLE_RIGHT */ { NULL, State::RIGHT_LIGHT_ON },
    /* PATH_FREE */ { NULL, State::RIGHT_LIGHT_ON },
    /* MOVE_FORWARD */ { moveForward, State::FORWARD_LIGHT_ON },
    /* MOVE_LEFT */ { moveLeft, State::LEFT_LIGHT_ON },
    /* MOVE_RIGHT */ { moveRight, State::RIGHT_LIGHT_ON },
    /* MOVE_BACKWARD */ { moveBackward, State::BACKWARD_LIGHT_ON },
    /* STOP */ { stopMotors, State::STILL_LIGHT_ON },
    /* LOW_LIGHT */ { NULL, State::RIGHT_LIGHT_ON },
    /* HIGH_LIGHT */ { switchOffLED, State::RIGHT } },
  /* AVOIDING_OBSTACLE_LEFT_LIGHT_ON */
  { /* OBSTACLE_LEFT */ { NULL, State::AVOIDING_OBSTACLE_LEFT_LIGHT_ON },
    /* OBSTACLE_RIGHT */ { NULL, State::AVOIDING_OBSTACLE_LEFT_LIGHT_ON },
    /* PATH_FREE */ { moveForward, State::FORWARD_LIGHT_ON },
    /* MOVE_FORWARD */ { NULL, State::AVOIDING_OBSTACLE_LEFT_LIGHT_ON },
    /* MOVE_LEFT */ { NULL, State::AVOIDING_OBSTACLE_LEFT_LIGHT_ON },
    /* MOVE_RIGHT */ { NULL, State::AVOIDING_OBSTACLE_LEFT_LIGHT_ON },
    /* MOVE_BACKWARD */ { NULL, State::AVOIDING_OBSTACLE_LEFT_LIGHT_ON },
    /* STOP */ { stopMotors, State::STILL_LIGHT_ON },
    /* LOW_LIGHT */ { NULL, State::AVOIDING_OBSTACLE_LEFT_LIGHT_ON },
    /* HIGH_LIGHT */ { switchOffLED, State::AVOIDING_OBSTACLE_LEFT } },
  /* AVOIDING_OBSTACLE_RIGHT_LIGHT_ON */
  { /* OBSTACLE_LEFT */ { NULL, State::AVOIDING_OBSTACLE_RIGHT_LIGHT_ON },
    /* OBSTACLE_RIGHT */ { NULL, State::AVOIDING_OBSTACLE_RIGHT_LIGHT_ON },
    /* PATH_FREE */ { moveForward, State::FORWARD_LIGHT_ON },
    /* MOVE_FORWARD */ { NULL, State::AVOIDING_OBSTACLE_RIGHT_LIGHT_ON },
    /* MOVE_LEFT */ { NULL, State::AVOIDING_OBSTACLE_RIGHT_LIGHT_ON },
    /* MOVE_RIGHT */ { NULL, State::AVOIDING_OBSTACLE_RIGHT_LIGHT_ON },
    /* MOVE_BACKWARD */ { NULL, State::AVOIDING_OBSTACLE_RIGHT_LIGHT_ON },
    /* STOP */ { stopMotors, State::STILL_LIGHT_ON },
    /* LOW_LIGHT */ { NULL, State::AVOIDING_OBSTACLE_RIGHT_LIGHT_ON },
    /* HIGH_LIGHT */ { switchOffLED, State::AVOIDING_OBSTACLE_RIGHT } }
};

int check_left_distance_sensor() {
  digitalWrite(LEFT_DISTANCE_SENSOR_TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(LEFT_DISTANCE_SENSOR_TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(LEFT_DISTANCE_SENSOR_TRIG_PIN, LOW);
  long duration = pulseIn(LEFT_DISTANCE_SENSOR_ECHO_PIN, HIGH, MAX_TIMEOUT_FOR_OBSTACLE_DETECTION_US);
  long distance = duration * SPEED_OF_SOUND;
  return distance;
}

int check_right_distance_sensor() {
  digitalWrite(RIGHT_DISTANCE_SENSOR_TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(RIGHT_DISTANCE_SENSOR_TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(RIGHT_DISTANCE_SENSOR_TRIG_PIN, LOW);
  long duration = pulseIn(RIGHT_DISTANCE_SENSOR_ECHO_PIN, HIGH, MAX_TIMEOUT_FOR_OBSTACLE_DETECTION_US);
  long distance = duration * SPEED_OF_SOUND;
  return distance;
}

Event check_obstacles() {
  int left_distance = check_left_distance_sensor();
  int right_distance = check_right_distance_sensor();

  if (left_distance > 0 && left_distance < MAX_DISTANCE_FOR_OBSTACLE_DETECTION_CM) {
    return Event::OBSTACLE_LEFT;
  } else if (right_distance > 0 && right_distance < MAX_DISTANCE_FOR_OBSTACLE_DETECTION_CM) {
    return Event::OBSTACLE_RIGHT;
  } else {
    return Event::PATH_FREE;
  }
}

Event check_light() {
  int light_value = analogRead(LIGHT_SENSOR_PIN);

  if (light_value > LIGHT_THRESHOLD) {
    return Event::LOW_LIGHT;
  } else {
    return Event::HIGH_LIGHT;
  }
}

QueueHandle_t events_queue;

const SensorFunction sensor_functions[] = { check_obstacles, check_light };

constexpr size_t SENSOR_FUNCTION_COUNT = sizeof(sensor_functions) / sizeof(sensor_functions[0]);

void task_sensors(void *_) {
  size_t sensor_index = 0;

  while (true) {
    Event event = sensor_functions[sensor_index]();

    xQueueSend(events_queue, &event, portMAX_DELAY);

    sensor_index = (sensor_index + 1) % SENSOR_FUNCTION_COUNT;

    vTaskDelay(pdMS_TO_TICKS(TASK_DELAY_MS));
  }
}

void task_commands(void *_) {
  while (true) {
    String s = Serial.readStringUntil('\n');

    if (s.equals("forward")) {
      Event event = Event::MOVE_FORWARD;
      xQueueSend(events_queue, &event, portMAX_DELAY);
    } else if (s.equals("backward")) {
      Event event = Event::MOVE_BACKWARD;
      xQueueSend(events_queue, &event, portMAX_DELAY);
    } else if (s.equals("left")) {
      Event event = Event::MOVE_LEFT;
      xQueueSend(events_queue, &event, portMAX_DELAY);
    } else if (s.equals("right")) {
      Event event = Event::MOVE_RIGHT;
      xQueueSend(events_queue, &event, portMAX_DELAY);
    } else if (s.equals("stop")) {
      Event event = Event::STOP;
      xQueueSend(events_queue, &event, portMAX_DELAY);
    }

    vTaskDelay(pdMS_TO_TICKS(TASK_DELAY_MS));
  }
}

State current_state = State::STILL;

State handleEvent(Event event) {
  Transition transition = sm[static_cast<uint8_t>(current_state)][static_cast<uint8_t>(event)];
  if (transition.action) {
    transition.action();
  }
  return transition.nextState;
}

String stateToReadable(State state) {
  switch (state) {
  case State::STILL:
    return "STILL";
  case State::FORWARD:
    return "FORWARD";
  case State::BACKWARD:
    return "BACKWARD";
  case State::LEFT:
    return "LEFT";
  case State::RIGHT:
    return "RIGHT";
  case State::AVOIDING_OBSTACLE_LEFT:
    return "AVOIDING_OBSTACLE_LEFT";
  case State::AVOIDING_OBSTACLE_RIGHT:
    return "AVOIDING_OBSTACLE_RIGHT";
  case State::STILL_LIGHT_ON:
    return "STILL_LIGHT_ON";
  case State::FORWARD_LIGHT_ON:
    return "FORWARD_LIGHT_ON";
  case State::BACKWARD_LIGHT_ON:
    return "BACKWARD_LIGHT_ON";
  case State::LEFT_LIGHT_ON:
    return "LEFT_LIGHT_ON";
  case State::RIGHT_LIGHT_ON:
    return "RIGHT_LIGHT_ON";
  case State::AVOIDING_OBSTACLE_LEFT_LIGHT_ON:
    return "AVOIDING_OBSTACLE_LEFT_LIGHT_ON";
  case State::AVOIDING_OBSTACLE_RIGHT_LIGHT_ON:
    return "AVOIDING_OBSTACLE_RIGHT_LIGHT_ON";
  default:
    return "UNKNOWN_STATE";
  }
}

String eventToReadable(Event event) {
  switch (event) {
  case Event::OBSTACLE_LEFT:
    return "OBSTACLE_LEFT";
  case Event::OBSTACLE_RIGHT:
    return "OBSTACLE_RIGHT";
  case Event::MOVE_FORWARD:
    return "MOVE_FORWARD";
  case Event::MOVE_LEFT:
    return "MOVE_LEFT";
  case Event::MOVE_RIGHT:
    return "MOVE_RIGHT";
  case Event::STOP:
    return "STOP";
  case Event::MOVE_BACKWARD:
    return "MOVE_BACKWARD";
  case Event::PATH_FREE:
    return "PATH_FREE";
  case Event::LOW_LIGHT:
    return "LOW_LIGHT";
  case Event::HIGH_LIGHT:
    return "HIGH_LIGHT";
  default:
    return "UNKNOWN_EVENT";
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(LEFT_DISTANCE_SENSOR_TRIG_PIN, OUTPUT);
  pinMode(LEFT_DISTANCE_SENSOR_ECHO_PIN, INPUT);
  pinMode(RIGHT_DISTANCE_SENSOR_TRIG_PIN, OUTPUT);
  pinMode(RIGHT_DISTANCE_SENSOR_ECHO_PIN, INPUT);
  pinMode(LIGHT_SENSOR_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);
  pinMode(L298N_LEFT_FORWARD_PIN, OUTPUT);
  pinMode(L298N_LEFT_BACKWARD_PIN, OUTPUT);
  pinMode(L298N_RIGHT_FORWARD_PIN, OUTPUT);
  pinMode(L298N_RIGHT_BACKWARD_PIN, OUTPUT);
  pinMode(L298N_LEFT_SPEED_PIN, OUTPUT);
  pinMode(L298N_RIGHT_SPEED_PIN, OUTPUT);

  analogWrite(L298N_LEFT_SPEED_PIN, MOTOR_SPEED);
  analogWrite(L298N_RIGHT_SPEED_PIN, MOTOR_SPEED);

  events_queue = xQueueCreate(QUEUE_SIZE, sizeof(Event));
  xTaskCreate(task_sensors, "Sensors Task", TASK_STACK_SIZE, NULL, 1, NULL);
  xTaskCreate(task_commands, "Commands Task", TASK_STACK_SIZE, NULL, 1, NULL);
}

void loop() {
  Event event;
  if (xQueueReceive(events_queue, &event, portMAX_DELAY) == pdTRUE) {
    State newState = handleEvent(event);
    if (newState != current_state) {
      Serial.print("Transitioning from ");
      Serial.print(stateToReadable(current_state));
      Serial.print(" to ");
      Serial.print(stateToReadable(newState));
      Serial.print(" on event ");
      Serial.println(eventToReadable(event));
    }
    current_state = newState;
  } else {
    Serial.println("No event received, staying in current state.");
  }
}