   {
     return;
   }
+  byte internal_address = Wire.read();
 
-  byte internal_address = Wire.read();
-  switch (internal_address)
+  if (internal_address == MOTOR1_ADDRESS || internal_address == MOTOR2_ADDRESS)
   {
-  case MOTOR1_ADDRESS:
-    motor_event(&motor1, receive_size - 1);
-    break;
-  case MOTOR2_ADDRESS:
-    motor_event(&motor2, receive_size - 1);
-    break;
-  default:
-    return;
+    if (!Wire.available())
+    {
+      return;
+    }
+    byte direction = Wire.read();
+
+    if (!Wire.available())
+    {
+      return;
+    }
+    byte speed = Wire.read();
+
+    struct Event e;
+    e.internal_address = internal_address;
+    e.direction = direction;
+    e.speed = speed;
+    buf.add(e);
   }
 }
 
-void motor_event(Motor *motor, int receive_size)
+void motor_event(Motor *motor, byte direction, byte speed)
 {
-  if (!Wire.available())
-  {
-    return;
-  }
-  byte direction = Wire.read();
-  if (!Wire.available())
-  {
-    return;
-  }
-  byte speed = Wire.read();
   if (0x01 == direction)
   {
     motor->forward(speed);

LCD lcd = LCD(LCD_TX_PIN, LCD_ROWS, LCD_COLUMNS);
char i2c_buffer[I2C_BUFFER_SIZE];

// Remember to use Arduino pins, not physical ones
Motor motor1 = Motor(2, 3);
Motor motor2 = Motor(4, 5);
volatile byte sensor_left, sensor_front, sensor_right;

void setup()
{

#include <Wire.h>
#include <RingBufCPP.h>
#include "LCD.h"
#include "Motor.h"

#define I2C_ADDRESS 0x1A
#define I2C_BUFFER_SIZE 32
#define MAX_NUM_ELEMENTS 30
#define LCD_TX_PIN 10
#define LCD_ROWS 4
#define LCD_COLUMNS 20
