diff --git a/CMakeLists.txt b/CMakeLists.txt
index e0ca8c1..ce65530 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,11 +3,12 @@ project(RoboUtils VERSION 0.1 LANGUAGES CXX)
 
 include(GNUInstallDirs)
 set(CMAKE_CXX_STANDARD 17)
-file(GLOB SOURCES src/*.cpp src/io/*.cpp src/comm/*.cpp)
+file(GLOB SOURCES src/*.cpp src/*/*.cpp)
 
-file(GLOB HEADERS include/*.h include/io/*.h include/comm/*.h include/chips/*.h)
+file(GLOB HEADERS include/*.h include/*/*.h)
 
 add_library(libroboutils ${SOURCES})
+add_compile_options(-Wall -Wextra -Werror -Wpedantic)
 
 target_include_directories(libroboutils PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
diff --git a/README.md b/README.md
index d2aefd3..af46a0a 100644
--- a/README.md
+++ b/README.md
@@ -2,44 +2,98 @@ RoboUtils
 =========
 C++ classes used for speeding up development of robots, suitable for Raspberry Pi hat KAMBot.
 
-Features classes for:
-* UDP
-* TCP
-* Thread safe variable access
-* Thread safe I2C (Linux only)
-* Measuring time
-* KAMBot periferies access
-* PID
-* Advanced logging with streams and logging over network
-
-## To be included
-* Drivers for KM2 stepper motor driver board with odometry calculations
-* PWM driver
-* Class for performing preprogrammed robot movement
+## Features classes for:
+
+### RoboUtils::IO namespace
+ * I2C thread-safe driver for RaspberryPi
+ * ADC using AD7997 chip
+ * GPIO using MCP23017 chip
+ * KM2 using KM2 motor controller board
+ * PWM using xxxx chip (currently not implemented)
+
+### RoboUtils::comm namespace
+ * UDP driver for transporting strings over network
+ * TCP driver (not reimplemented)
+ 
+### RoboUtils::util namespace
+ * Thread safe variable access
+ * Measuring time
+ * Convenient string to list manipulation utility (usable for NMEA parser)
+
+### RoboUtils namespace
+
+ * PID
+ * Advanced logging with streams and logging over network
+
+## What will be NEVER featured:
+
+ * The NMEA parser (part of student semestral project)
+ * The analog preprocessing of line
+ * The robot wheel kinematics (yep, the hell!)
+ * The robot controller for line following
+ * Yep, any part of code the students should develop in the labs in their way that will be examined
+
 
 ## Installation
 
-```
-git clone https://github.com/matoushybl/RoboUtils.git
+### Using global liborary
+
+```bash
+git clone https://student.robotika.ceitec.vutbr.cz/VYUKA/BPC-PRP/RoboUtils.git
 cd RoboUtils
 cmake . && make install
 ```
 
 In your project's `CMakeLists.txt`:
-```
-cmake_minimum_required(VERSION 3.7)
-project(MyCoolRobot)
 
+```cMake
 find_package(RoboUtils REQUIRED)
-set(CMAKE_CXX_STANDARD 14)
-SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
 
-add_executable(MyCoolRobot main.cpp)
+add_executable(<appname> <... src/main.cpp etc ...>)
+
+target_link_libraries(<appname> libroboutils)
+```
+
+WARNING: Any future change in API of the lib will disturb all your projects you are using it. The devil who owns 
+library root will rule all your code.
+
 
-target_link_libraries(MyCoolRobot libroboutils)
+### Using specific branch in Your project (dependency hell issue)
+
+Use this in the case you want to maintain specific version of lib (ie you do not want future library changes to be 
+ported to your code) This is the case when the library API is changing and you need to maintain a lot of projects 
+that depends on it. The updating root library from previous example might be a nightmare.
+
+Assume you have project with `CMakeLists.txt` in root directory of you GITted project. Assume you have all sources 
+in `src` subdirectory and you want all libraries to be sourced from `lib` subdirectory (that is some standard style). 
+If you using different style, you need to update the example accordingly.
+
+Run this in git root of your project:
+
+```bash
+git submodule add https://student.robotika.ceitec.vutbr.cz/VYUKA/BPC-PRP/RoboUtils.git lib/RoboUtils
+```
+
+next, add following lines into your CMakeLists ()
+
+```CMake
+find_library(libroboutils lib/RoboUtils)
+
+add_subdirectory(lib/RoboUtils)
+add_executable(<appname> <... src/main.cpp etc ...>)
+
+target_link_libraries(<appname> PRIVATE libroboutils <... other libs ...>)
 ```
 
-## Contributors
-Ing. Frantisek Burian Ph.D.
+with this setup you will have fixed version of library unlike you update it by using ```git pull``` from the ```lib/RoboUtils``` 
+subdirectory. Any future changes to API of the lib will not disturb your project unlike you want to. You will be a god of your code.
+
+## Authors
+
+This library has been originally written by Matous Hybl [hyblmatous@gmail.com](mailto:hyblmatous@gmail.com) in 2020. We have very thanks for his work.
+
+Some parts has been later reimplemented by Frantisek Burian - bufran _at_ seznam.cz from 2022 for the sake of 
+demonstrating simple API design, and good programming skills for our BPC-UDP course on Brno university of Technology.  
+
+
 
-Matous Hybl [hyblmatous@gmail.com](mailto:hyblmatous@gmail.com)
diff --git a/include/roboutils/Log.h b/include/roboutils/Log.h
index c3e7f1b..d740bdb 100644
--- a/include/roboutils/Log.h
+++ b/include/roboutils/Log.h
@@ -1,70 +1,54 @@
-#include <utility>
-
+#pragma once
 //
 // Created by Matous Hybl on 2018-10-12.
 //
 
-#ifndef FIRMWARE_LOG_H
-#define FIRMWARE_LOG_H
-
+#include <utility>
 #include <iostream>
 #include <string>
-#include "roboutils/comm/UDP.h"
+#include <functional>
+#include <sstream>
+
+#include <roboutils/comm/UDP.h>
 
 namespace RoboUtils {
-    using namespace std;
 
     class Log {
-    public:
-
-        Log(string level) {
-            this->level = std::move(level);
-        }
-
-        static Log error();
-
-        static Log warning();
+    private:
+        const char * level_{nullptr};
 
-        static Log info();
+        class LogLine : public std::stringstream {
+        public:
+            explicit LogLine(const Log & parent);
+            LogLine(LogLine &line);
+            ~LogLine() override;
+        };
 
-        static Log debug();
+    public:
+        static const Log error;
+        static const Log warning;
+        static const Log info;
+        static const Log debug;
 
-        static void setPath(string path);
+        static std::function<void(const std::string &)> writer;
 
-        static void setRemoteTarget(string address);
+        explicit constexpr Log(const char *level)
+           : level_(level)
+        {
+        }
 
         template<class T>
-        Log &operator<<(const T &value) {
-            buffer << value;
-
-            return *this;
+        LogLine operator<<(const T &value) const {
+            LogLine line(*this);
+            line << value;
+            return line;
         }
 
-        Log &operator<<(std::ostream &(*manipulator)(std::ostream &)) {
-            if (manipulator == static_cast<std::ostream &(*)(std::ostream &)>(std::flush) ||
-                manipulator == static_cast<std::ostream &(*)(std::ostream &)>(std::endl)) {
-                Log::log(level, buffer.str());
-
-                buffer.flush();
-            }
-
-            return *this;
+        LogLine line() const {
+            return LogLine(*this);
         }
 
-    private:
-        stringstream buffer;
-        string level;
-
-        static string path;
-        static string address;
-        static uint16_t port;
-        static COMM::UDP udp;
-
-        static void log(const string& level, const string& message);
-
-        static string time();
     };
-};
 
 
-#endif //FIRMWARE_LOG_H
+}
diff --git a/include/roboutils/PIDController.h b/include/roboutils/PIDController.h
index e383099..aaec43b 100644
--- a/include/roboutils/PIDController.h
+++ b/include/roboutils/PIDController.h
@@ -24,7 +24,7 @@ namespace RoboUtils {
         unsigned int sampleTimeMs = 0;
         float absoluteMaximumAction = 0;
     };
-};
+}
 
 
 #endif //FIRMWARE_PIDCONTROLLER_H
diff --git a/include/roboutils/Variable.h b/include/roboutils/Variable.h
deleted file mode 100644
index 9a978d6..0000000
--- a/include/roboutils/Variable.h
+++ /dev/null
@@ -1,41 +0,0 @@
-//
-// Created by Matous Hybl on 2018-11-14.
-//
-
-#ifndef FIRMWARE_VARIABLE_H
-#define FIRMWARE_VARIABLE_H
-
-#include <mutex>
-
-namespace RoboUtils {
-    template<typename T>
-    class Variable {
-    public:
-        explicit Variable(T value) {
-            this->value = value;
-        }
-
-        void set(T value);
-
-        T get();
-
-    private:
-        T value;
-        std::mutex mutex;
-    };
-
-    template<typename T>
-    void Variable<T>::set(T value) {
-        std::lock_guard<std::mutex> lock(mutex);
-        this->value = value;
-    }
-
-    template<typename T>
-    T Variable<T>::get() {
-        std::lock_guard<std::mutex> lock(mutex);
-        return value;
-    }
-};
-
-
-#endif //FIRMWARE_VARIABLE_H
diff --git a/include/roboutils/chips/ad799x.h b/include/roboutils/chips/ad799x.h
index 40d23e1..699aa75 100644
--- a/include/roboutils/chips/ad799x.h
+++ b/include/roboutils/chips/ad799x.h
@@ -180,18 +180,26 @@ namespace RoboUtils::Chips::Ad799X {
 
     namespace CONFIG {
 
-// generic for group A, C, D*
-#define AD799X_CONFIG_CH(ch)               (1 << ((ch)+4))
-#define AD799X_CONFIG_CH_ALL               (0xFF << 4)
-#define AD799X_CONFIG_FLTR                 (1 << 3)
-#define AD799X_CONFIG_ALERTPIN             (3 << 1)
-#define AD799X_CONFIG_ALERTPIN_DISABLED    (0 << 1)
-#define AD799X_CONFIG_ALERTPIN_BUSY        (1 << 1)
-#define AD799X_CONFIG_ALERTPIN_ALERT       (2 << 1)
-#define AD799X_CONFIG_ALERTPIN_RESET       (3 << 1)
-#define AD799X_CONFIG_ALERTPOL             (1 << 0)
-#define AD799X_CONFIG_ALERTPOL_LOW         (0 << 0)
-#define AD799X_CONFIG_ALERTPOL_HIGH        (1 << 0)
+        static constexpr uint16_t ToCh(int ch) {
+            return 1 << (ch + 4);
+        }
+
+        // generic for group A, C, D*
+        enum : uint16_t {
+            ALERTPOL = 1 << 0,
+            ALERTBUSY = 1 << 1,
+            ALERTEN = 1 << 2,
+            FLTR = 1 << 3,
+            CH1 = ToCh(0),
+            CH2 = ToCh(1),
+            CH3 = ToCh(2),
+            CH4 = ToCh(3),
+            CH5 = ToCh(4),
+            CH6 = ToCh(5),
+            CH7 = ToCh(6),
+            CH8 = ToCh(7),
+            CH = 0xFF << 4,
+        };
 
 // group B only
 #define AD7991_CONFIG_CH(ch)               (1 << ((ch)+4))
diff --git a/include/roboutils/chips/mcp23017.h b/include/roboutils/chips/mcp23017.h
index b57828b..c62548f 100644
--- a/include/roboutils/chips/mcp23017.h
+++ b/include/roboutils/chips/mcp23017.h
@@ -87,7 +87,7 @@ namespace RoboUtils::Chips::Mcp23017 {
         };
 
     }
-};
+}
 
 
 
diff --git a/include/roboutils/comm/Serial.h b/include/roboutils/comm/Serial.h
new file mode 100644
index 0000000..aab84c2
--- /dev/null
+++ b/include/roboutils/comm/Serial.h
@@ -0,0 +1,60 @@
+#pragma once
+/*
+ MIT License
+
+Copyright (c) 2019-2021 Matous Hybl
+Copyright (c) 2022 Frantisek Burian
+
+  Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated
+documentation files (the "Software"), to deal in the Software without restriction, including without limitation
+the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and
+to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+
+  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
+WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#include <cstdint>
+#include <string>
+#include <vector>
+#include <tuple>
+
+namespace RoboUtils::COMM {
+
+    class Serial {
+    public:
+        ~Serial();
+
+        void open(const std::string &file);
+
+        void configure();
+
+        void send(const uint8_t *buffer, std::size_t size) const;
+
+        /// \brief Send text data to specified host
+        ///
+        /// \param data text to be sent
+        void sendStr(const std::string &data) const {
+          send(reinterpret_cast<const uint8_t *>(data.c_str()), data.length());
+        }
+
+        /// \brief Receive data
+        ///
+        /// \return data
+        std::vector<uint8_t> receive() const;
+
+        /// \brief Receive text
+        ///
+        /// \return auto text
+        std::string receiveStr() const;
+
+    private:
+        int fileDescriptor = 0;
+    };
+
+}
\ No newline at end of file
diff --git a/include/roboutils/comm/UDP.h b/include/roboutils/comm/UDP.h
index 3bcdfe2..f37b979 100644
--- a/include/roboutils/comm/UDP.h
+++ b/include/roboutils/comm/UDP.h
@@ -29,23 +29,46 @@ namespace RoboUtils::COMM {
 
     class UDP {
     public:
+        /// \brief Create the UDP object
         UDP();
 
+        /// \brief Destroy the UDP object
         ~UDP();
 
-        void bind(uint16_t port);
+        /// \brief Bind to specified UDP port
+        ///
+        /// \param port the port to bind to (default is automatic)
+        void bind(uint16_t port = 0);
 
+        /// \brief Send data to specified host
+        ///
+        /// \param host the host identification ip/port
+        /// \param buffer the data to send
+        /// \param size size of data to send
         void send(const std::string &host, const uint8_t *buffer, std::size_t size) const;
 
+        /// \brief Send text data to specified host
+        ///
+        /// \param host the host identification ip/port
+        /// \param data text to be sent
         void sendStr(const std::string &host, const std::string &data) const
         {
             send(host, reinterpret_cast<const uint8_t*>(data.c_str()), data.length());
         }
 
+        /// \brief Return if any packet is waiting for receive
+        ///
+        /// \return true, if there is waiting packet to receive
         bool available() const;
 
+        /// \brief Receive data from remote
+        ///
+        /// \return auto [remote, data]
         std::tuple<std::string, std::vector<uint8_t>> receive() const;
 
+        /// \brief Receive text from remote
+        ///
+        /// \return auto [remote, text]
         std::tuple<std::string, std::string> receiveStr() const;
 
         bool bound{false};
diff --git a/include/roboutils/io/ADC.h b/include/roboutils/io/ADC.h
index b267132..804d33a 100644
--- a/include/roboutils/io/ADC.h
+++ b/include/roboutils/io/ADC.h
@@ -21,18 +21,51 @@ SOFTWARE.
 
 #include <roboutils/io/I2C.h>
 
+#include <vector>
+#include <map>
+
 namespace RoboUtils::IO {
 
     class ADC {
     public:
-        explicit ADC(I2C *i2c);
+        explicit ADC(const I2C &i2c);
+
+        ///-------------------------------------------------------------------------------------------------------------
+        /// \brief operator that informs if bus is already opened
+        ///
+        /// \return true, if chip is already present
+        operator bool() const;
+
+        ///-------------------------------------------------------------------------------------------------------------
+        /// \brief Associated bus
+        ///
+        /// \return the associated bus
+        const I2C &bus() const;
+
+        ///-------------------------------------------------------------------------------------------------------------
+        /// \brief Chip address
+        ///
+        /// \return the current chip address
+        int chip() const;
+
+        /// \brief Measure single value on ADC channel
+        ///
+        /// \param channel the channel number (0-7)
+        /// \return the map with measured value
+        std::map<int, uint16_t> Mode2Measure(int channel);
+
+        /// \brief Measure multiple values on multiple channels
+        ///
+        /// \param channels all channels to be measured
+        /// \return the map set up with measured values
+        std::map<int, uint16_t> Mode2Measure(const std::vector<int>& channels);
 
-        uint16_t readChannel(uint8_t channel);
+        void setCycleMode(int divisor);
 
     private:
-        I2C *i2c;
+        const I2C &i2c_;
 
-        int chipAddress;
+        const int chipAddress_;
     };
 
     class adc_error : public std::logic_error {
diff --git a/include/roboutils/io/GPIO.h b/include/roboutils/io/GPIO.h
index 58ba2f4..40becdc 100644
--- a/include/roboutils/io/GPIO.h
+++ b/include/roboutils/io/GPIO.h
@@ -29,8 +29,10 @@ namespace RoboUtils::IO {
     /// \code
     ///  auto RUN = Pin::PA7;
     ///
-    ///  I2C bus{"/dev/i2c-1"};
-    ///  GPIO gpio{ &bus };
+    ///  I2C bus;
+    ///  GPIO gpio{bus};
+    ///
+    ///  bus.open("/dev/i2c-1");
     ///
     ///  gpio.output(Pin::PA1 | Pin::PA2 | Pin::PA3);        // set PA1...PA3 to output
     ///  gpio.input(RUN | Pin::PA6, true);                   // set pins PA7 and PA6 to input with pullup
@@ -52,7 +54,31 @@ namespace RoboUtils::IO {
         ///
         /// \param i2c the bus to communicate over
         /// \param chipIndex the index of the chip on bus
-        explicit GPIO(I2C *i2c, int chipIndex = 0);
+        explicit GPIO(const I2C &i2c, int chipIndex = 0);
+
+        ///-------------------------------------------------------------------------------------------------------------
+        /// \brief operator that informs if bus is already opened
+        ///
+        /// \return true, if chip is already present
+        operator bool() const;
+
+        ///-------------------------------------------------------------------------------------------------------------
+        /// \brief Associated bus
+        ///
+        /// \return the associated bus
+        const I2C &bus() const;
+
+        ///-------------------------------------------------------------------------------------------------------------
+        /// \brief Chip address
+        ///
+        /// \return the current chip address
+        int chip() const;
+
+        ///-------------------------------------------------------------------------------------------------------------
+        /// \brief Set chip address (late initialization)
+        ///
+        /// \param address the desired chip address
+        void set_chip(int address);
 
         ///-------------------------------------------------------------------------------------------------------------
         /// \brief Set selected pins to input mode
@@ -109,10 +135,10 @@ namespace RoboUtils::IO {
 
     private:
         /// the bus
-        I2C *i2c = nullptr;
+        const I2C &i2c_;
 
         /// original chip address
-        int chipAddress;
+        int chipAddress_;
     };
 
     class gpio_error : public std::logic_error {
diff --git a/include/roboutils/io/I2C.h b/include/roboutils/io/I2C.h
index b8c50b3..b99cff8 100644
--- a/include/roboutils/io/I2C.h
+++ b/include/roboutils/io/I2C.h
@@ -28,21 +28,37 @@ SOFTWARE.
 #include <cstdint>
 
 namespace RoboUtils::IO {
+    enum class Endian {
+        Little,
+        Big
+    };
+
     class I2C {
     public:
 
-        ///-------------------------------------------------------------------------------------------------------------
-        /// \brief construct the bus accessor
-        ///
-        /// \param busFile filename of the linux bus driver
-        explicit I2C(const std::string &busFile = "/dev/i2c-1");
-
         ///-------------------------------------------------------------------------------------------------------------
         /// \brief destructor of the bus accessor
         ///
         /// this destructs all system resources
         ~I2C();
 
+        ///-------------------------------------------------------------------------------------------------------------
+        /// \brief operator that informs if bus is already opened
+        ///
+        /// \return true, if bus is correctly opened
+        operator bool() const;
+
+        ///-------------------------------------------------------------------------------------------------------------
+        /// \brief open the specified bus
+        ///
+        /// \param busFile filename of the linux bus driver
+        /// \return true if succeeded and bus is open
+        bool open(const std::string &busFile= "/dev/i2c-1");
+
+        ///-------------------------------------------------------------------------------------------------------------
+        /// \brief get current bus file
+        std::string bus() const;
+
         ///-------------------------------------------------------------------------------------------------------------
         /// \brief Write register value on the bus as LittleEndian
         ///
@@ -56,7 +72,7 @@ namespace RoboUtils::IO {
         /// \param value the value to be written
         /// \param littleEndian the endianity of he transaction
         /// \return true if there is no error during transfer
-        template<typename T> bool write(int chipAddress, int registerAddress, T value, bool littleEndian=true) const;
+        template<typename T> bool write(int chipAddress, int registerAddress, T value, Endian endian=Endian::Little) const;
 
         ///-------------------------------------------------------------------------------------------------------------
         /// \brief Write an array of register values on the bus as LittleEndian
@@ -71,7 +87,7 @@ namespace RoboUtils::IO {
         /// \param array the array of values to be written
         /// \param count the array size
         /// \return true if there is no error during transfer
-        template<typename T> bool write(int chipAddress, int registerAddress, const T *array, int count, bool littleEndian=true) const;
+        template<typename T> bool write(int chipAddress, int registerAddress, const T *array, int count, Endian endian=Endian::Little) const;
 
         ///-------------------------------------------------------------------------------------------------------------
         /// \brief Read register value from the bus as LittleEndian
@@ -85,7 +101,7 @@ namespace RoboUtils::IO {
         /// \param registerAddress the address of the register on the chip
         /// \param value the value will be filled wirh read data
         /// \return true if there is no error during transfer
-        template<typename T> bool read(int chipAddress, int registerAddress, T *value, bool littleEndian=true) const;
+        template<typename T> bool read(int chipAddress, int registerAddress, T *value, Endian endian=Endian::Little) const;
 
         ///-------------------------------------------------------------------------------------------------------------
         /// \brief Read array of register values from the bus as LittleEndian
@@ -100,16 +116,18 @@ namespace RoboUtils::IO {
         /// \param array the array of values to be filled with returned data
         /// \param count the array size
         /// \return true if there is no error during transfer
-        template<typename T> bool read(int chipAddress, int registerAddress, T *array, int count, bool littleEndian=true) const;
+        template<typename T> bool read(int chipAddress, int registerAddress, T *array, int count, Endian endian=Endian::Little) const;
 
 
         template<typename T>
-        bool update(uint8_t chipAddress, uint8_t registerAddress, T setBits, T clearBits, T toggleBits, bool littleEndian=true) const;
+        bool update(uint8_t chipAddress, uint8_t registerAddress, T setBits, T clearBits, T toggleBits, Endian endian=Endian::Little) const;
 
     private:
         /// the file descriptor
         int i2cDescriptor;
 
+        std::string bus_; // filename of bus
+
         /// the thread-guarding mutex
         mutable std::mutex mutex;
 
@@ -122,7 +140,7 @@ namespace RoboUtils::IO {
         /// \param r
         /// \param rn
         /// \return true if there is no error during transfer
-        bool transact_(int addr, uint8_t *w, uint32_t wn, uint8_t *r, uint32_t rn) const;
+        bool transact_(int addr, uint8_t *w, int wn, uint8_t *r, int rn) const;
 
         ///-------------------------------------------------------------------------------------------------------------
         /// \brief
@@ -134,7 +152,7 @@ namespace RoboUtils::IO {
         /// \param count the array size
         /// \param littleEndian the endianity of he transaction
         /// \return true if there is no error during transfer
-        bool write_(int chipAddress, int registerAddress,  const uint8_t *data, int size, int count, bool litleEndian) const;
+        bool write_(int chipAddress, int registerAddress,  const uint8_t *data, int size, int count, Endian endian=Endian::Little) const;
 
         ///-------------------------------------------------------------------------------------------------------------
         /// \brief
@@ -146,7 +164,7 @@ namespace RoboUtils::IO {
         /// \param count the array size
         /// \param littleEndian the endianity of he transaction
         /// \return true if there is no error during transfer
-        bool read_(int chipAddress, int registerAddress,  uint8_t *data, int size, int count, bool litleEndian) const;
+        bool read_(int chipAddress, int registerAddress,  uint8_t *data, int size, int count, Endian endian=Endian::Little) const;
     };
 
     class i2c_error : public std::logic_error {
diff --git a/include/roboutils/io/KM2.h b/include/roboutils/io/KM2.h
index 7fcb181..3148d36 100644
--- a/include/roboutils/io/KM2.h
+++ b/include/roboutils/io/KM2.h
@@ -25,20 +25,58 @@ namespace RoboUtils::IO {
 
     class KM2 {
     public:
-        explicit KM2(I2C *aBus, int aChipAddress = 0x71);
-
+        explicit KM2(const I2C &aBus, int aChipAddress = 0x71);
+
+        ///-------------------------------------------------------------------------------------------------------------
+        /// \brief operator that informs if bus is already opened
+        ///
+        /// \return true, if chip is already present
+        operator bool() const;
+
+        ///-------------------------------------------------------------------------------------------------------------
+        /// \brief Associated bus
+        ///
+        /// \return the associated bus
+        const I2C &bus() const;
+
+        ///-------------------------------------------------------------------------------------------------------------
+        /// \brief Chip address
+        ///
+        /// \return the current chip address
+        int chip() const;
+
+        /// \brief Set desired speed for both of the motors
+        ///
+        /// \param left left motor speed
+        /// \param right right motor speed
         void drive(int left, int right) const;
 
+        /// \brief Ask for odometry
+        ///
+        /// \code
+        ///   auto [ left, right ] = odo();
+        /// \endcode
+        ///
+        /// \return current odometry measurement
         std::tuple<int,int> odometry() const;
 
+        /// \brief Set desired speed for both of the motors and ask for odometry
+        ///
+        /// \code
+        ///   auto [ left, right ] = driveodo(3,5);
+        /// \endcode
+        ///
+        /// \param left left motor speed
+        /// \param right right motor speed
+        /// \return current odometry measurement
         std::tuple<int,int> driveodo(int left, int right) const;
 
         void setAddress(int newaddr, bool bcast) const;
 
     private:
-        I2C *bus;
+        const I2C &bus_;
 
-        int chipAddress;
+        const int chipAddress_;
     };
 
     class km2_error : public std::logic_error {
diff --git a/include/roboutils/math.h b/include/roboutils/math.h
deleted file mode 100644
index 503a0ff..0000000
--- a/include/roboutils/math.h
+++ /dev/null
@@ -1,41 +0,0 @@
-//
-// Created by Matous Hybl on 02/10/2018.
-//
-
-#include <cstdint>
-
-#ifndef FIRMWARE_MATH_H
-#define FIRMWARE_MATH_H
-
-namespace RoboUtils {
-#define PI 3.14159f
-
-    template<typename T>
-    static inline constexpr T saturate(const T value, const T minimum, const T maximum)
-    {
-        if (value < minimum)
-            return minimum;
-        if (value > maximum)
-            return maximum;
-        return value;
-    }
-
-    template<typename T>
-    static inline constexpr T saturate(const T value, const T maximum)
-    {
-        if (value < -maximum)
-            return -maximum;
-        if (value > maximum)
-            return maximum;
-        return value;
-    }
-
-    template<typename T>
-    static inline constexpr T ramp(const T value, const T desired, const T limit)
-    {
-        return value + saturate(desired - value, -limit, limit);
-    }
-
-};
-
-#endif //FIRMWARE_MATH_H
diff --git a/include/roboutils/strings.h b/include/roboutils/strings.h
deleted file mode 100644
index 0682f07..0000000
--- a/include/roboutils/strings.h
+++ /dev/null
@@ -1,12 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-
-namespace RoboUtils {
-
-    std::vector<std::string> split(const std::string& text, char delim);
-
-    std::string join(const std::vector<std::string>& data, const std::string& delim);
-
-}
\ No newline at end of file
diff --git a/include/roboutils/threading.h b/include/roboutils/threading.h
deleted file mode 100644
index ecca672..0000000
--- a/include/roboutils/threading.h
+++ /dev/null
@@ -1,14 +0,0 @@
-//
-// Created by Matous Hybl on 2018-11-14.
-//
-
-#ifndef FIRMWARE_THREADING_H
-#define FIRMWARE_THREADING_H
-
-#include <functional>
-
-namespace RoboUtils {
-    void repeatAsynchronously(unsigned int periodMs, const std::function<void(void)> &fun);
-};
-
-#endif //FIRMWARE_THREADING_H
diff --git a/include/roboutils/util/locales.h b/include/roboutils/util/locales.h
new file mode 100644
index 0000000..55eb4fe
--- /dev/null
+++ b/include/roboutils/util/locales.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include <string>
+
+namespace RoboUtils {
+
+    template <int LC>
+    class Locale {
+    public:
+        explicit Locale(const char *loc = "C");
+
+        ~Locale();
+
+    private:
+        const std::string loc_;
+    };
+
+
+    using NumericLocale = Locale<LC_NUMERIC>;
+    using TimeLocale = Locale<LC_TIME>;
+}
diff --git a/include/roboutils/util/mathematic.h b/include/roboutils/util/mathematic.h
new file mode 100644
index 0000000..da21adc
--- /dev/null
+++ b/include/roboutils/util/mathematic.h
@@ -0,0 +1,66 @@
+//
+// Created by Matous Hybl on 02/10/2018.
+//
+#pragma once
+
+#include <cstdint>
+
+namespace RoboUtils {
+
+    /// \brief Saturate value between minimum and maximum
+    ///
+    /// \code
+    ///   int64_t value = 100000;
+    ///   value = saturate(value, -200, 100);   // = 100
+    /// \endcode
+    ///
+    /// \tparam T any countable value type (ints or floats of any size)
+    /// \param value the value to be saturated
+    /// \param minimum lower limit
+    /// \param maximum upper limit
+    /// \return the saturated value
+    template<typename T>
+    static inline constexpr T saturate(const T value, const T minimum, const T maximum)
+    {
+        if (value < minimum)
+            return minimum;
+        if (value > maximum)
+            return maximum;
+        return value;
+    }
+
+    /// \brief Saturate value to symmetrical limit
+    ///
+    /// \code
+    ///   int64_t value = -100000;
+    ///   value = saturate(value, 100);   // = -100
+    /// \endcode
+    ///
+    /// \tparam T any countable value type (ints or floats of any size)
+    /// \param value the value to be saturated
+    /// \param maximum the limit
+    /// \return the saturated value
+    template<typename T>
+    static inline constexpr T saturate(const T value, const T maximum)
+    {
+        if (value < -maximum)
+            return -maximum;
+        if (value > maximum)
+            return maximum;
+        return value;
+    }
+
+    /// \brief Tracking speed limiter
+    ///
+    /// \tparam T any countable value type (ints or floats of any size)
+    /// \param value actual value
+    /// \param desired the desired value the user wants to have
+    /// \param limit the maximum step per regulation
+    /// \return saturated actual value
+    template<typename T>
+    static inline constexpr T ramp(const T value, const T desired, const T limit)
+    {
+        return value + saturate(desired - value, -limit, limit);
+    }
+
+}
diff --git a/include/roboutils/util/strings.h b/include/roboutils/util/strings.h
new file mode 100644
index 0000000..25017e8
--- /dev/null
+++ b/include/roboutils/util/strings.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+namespace RoboUtils {
+
+    /// \brief Split the supplied string by delimiter
+    ///
+    /// \code
+    ///   std::string data{"AHOJ:JOHA:OUHA:HAHA:NOHA"};
+    ///   auto result = RoboUtils::split(data, ':');    //  == { "AHOJ", "JOHA", "OUHA", "HAHA", "NOHA"}
+    /// \endcode
+    ///
+    /// \param text string to be splitted
+    /// \param delim the delimiter
+    /// \return splitted vector of string
+    std::vector<std::string> split(const std::string& text, char delim);
+
+    /// \brief Join the supplied set of strings with delimiter
+    ///
+    /// \code
+    ///   std::vector<std::string> data{ "AHOJ", "JOHA", "OUHA", "HAHA", "NOHA"};
+    ///   auto result = RoboUtils::join(data, ":");    //  == "AHOJ:JOHA:OUHA:HAHA:NOHA
+    /// \endcode
+    ///
+    /// \param data the list of strings
+    /// \param delim the delimiter
+    /// \return joined vector of string
+    std::string join(const std::vector<std::string>& data, const std::string& delim);
+
+    std::string join(const std::vector<std::string>::iterator & begin, const std::vector<std::string>::iterator & end, const std::string& delim);
+
+    std::string to_fixed(double d, int decimals);
+
+    void eraseBefore(std::string &s, char c);
+    void eraseBefore(std::string &s, const std::string &c);
+
+}
\ No newline at end of file
diff --git a/include/roboutils/util/swift.h b/include/roboutils/util/swift.h
new file mode 100644
index 0000000..a67adb3
--- /dev/null
+++ b/include/roboutils/util/swift.h
@@ -0,0 +1,7 @@
+#pragma once
+
+// Original tools make for compatibility with swift by Matous Hybl
+
+#define var auto
+#define let const auto
+#define guard(condition) if (condition) {}
\ No newline at end of file
diff --git a/include/roboutils/util/threading.h b/include/roboutils/util/threading.h
new file mode 100644
index 0000000..6337f62
--- /dev/null
+++ b/include/roboutils/util/threading.h
@@ -0,0 +1,25 @@
+//
+// Created by Matous Hybl on 2018-11-14.
+//
+
+#pragma once
+
+#include <functional>
+
+namespace RoboUtils {
+
+    /// \brief Call this function periodically from other thread
+    ///
+    /// when the function returns false, the thread stops.
+    ///
+    /// The time behavior is undefined when the duration of the function is longer than the period
+    ///
+    /// \code
+    ///   repeatAsynchronously(100, [](){ printf("I still live !\r\n"); return true; });
+    /// \endcode
+    /// \param periodMs period to call the function
+    /// \param fun the function to call
+    void repeatAsynchronously(unsigned int periodMs, const std::function<bool(void)> &fun);
+
+}
+
diff --git a/include/roboutils/util/timing.h b/include/roboutils/util/timing.h
new file mode 100644
index 0000000..19ff30c
--- /dev/null
+++ b/include/roboutils/util/timing.h
@@ -0,0 +1,28 @@
+//
+// Created by Matous Hybl on 2018-10-14.
+//
+
+#pragma once
+
+
+namespace RoboUtils {
+
+    /// \brief Get curreent msecs
+    ///
+    /// \return coumt of milliseconds
+    long long millis();
+
+    /// \brief Do nothing for specified time
+    ///
+    /// \param ms count of msecs to do notning
+    void delay(long ms);
+
+
+    /// \brief Check if there was elapsed time from last event
+    ///
+    /// \param last_event the time of last elapsed event
+    /// \param delay time in ms between events
+    /// \param ms reference timestamp or -1 to get actual timestamp from system
+    /// \return true, if the timeout elapsed (event time is reset]
+    bool expired(long long *last_event, long delay, long long ms = -1);
+}
diff --git a/include/roboutils/util/variable.h b/include/roboutils/util/variable.h
new file mode 100644
index 0000000..8afb08b
--- /dev/null
+++ b/include/roboutils/util/variable.h
@@ -0,0 +1,36 @@
+//
+// Created by Matous Hybl on 2018-11-14.
+//
+
+#pragma once
+
+#include <mutex>
+
+namespace RoboUtils {
+
+    template<typename T>
+    class Variable {
+    public:
+        explicit Variable(T val) {
+            value = val;
+        }
+
+        void set(T val) {
+            std::lock_guard<std::mutex> lock(mutex);
+            value = val;
+        }
+
+        T get() const {
+            std::lock_guard<std::mutex> lock(mutex);
+            return value;
+        }
+
+    private:
+        T value;
+        mutable std::mutex mutex;
+    };
+
+}
+
+
+
diff --git a/include/roboutils/utils.h b/include/roboutils/utils.h
deleted file mode 100644
index 0fc6e6d..0000000
--- a/include/roboutils/utils.h
+++ /dev/null
@@ -1,27 +0,0 @@
-//
-// Created by Matous Hybl on 2018-10-14.
-//
-
-#ifndef FIRMWARE_UTILS_H
-#define FIRMWARE_UTILS_H
-
-#include <chrono>
-#include <unistd.h>
-
-namespace RoboUtils {
-#define var auto
-#define let const auto
-#define guard(condition) if (condition) {}
-
-    inline long long millis() {
-        return std::chrono::duration_cast<std::chrono::milliseconds>(
-                std::chrono::system_clock::now().time_since_epoch()
-        ).count();
-    }
-
-    inline void delay(long ms) {
-        usleep(ms * 1000);
-    }
-};
-
-#endif //FIRMWARE_UTILS_H
diff --git a/src/Log.cpp b/src/Log.cpp
index d7ec24a..37801a1 100644
--- a/src/Log.cpp
+++ b/src/Log.cpp
@@ -1,75 +1,48 @@
 //
 // Created by Matous Hybl on 2018-10-12.
 //
-
-#include <cstdio>
-#include <sstream>
-#include <fstream>
-#include <utility>
 #include <roboutils/Log.h>
-#include <roboutils/utils.h>
 
 using namespace RoboUtils;
 
-    string Log::path = "";
-    string Log::address = "";
-    COMM::UDP Log::udp = COMM::UDP();
-
-    void Log::setPath(string path) {
-        ostringstream buffer;
-        buffer << path << "/LBot-" << Log::time() << ".log";
-        Log::path = buffer.str();
-    }
-
-    void Log::setRemoteTarget(string address) {
-        Log::address = std::move(address);
-
-        cout << "Logging address was set to: " << Log::address;
-    }
-
-    void Log::log(const string& level, const string& message) {
-        std::ostringstream buffer;
-
-        buffer << "[" << level << "]: " << Log::time() << " - " << message;
-
-        cout << buffer.str() << endl;
-
-        if (!path.empty()) {
-            ofstream file(path, ios_base::app);         // RAII: file will be closed automatically
-            file << buffer.str() << endl;
-        }
+const Log Log::error = Log("ERROR");
+const Log Log::warning = Log("WARNING");
+const Log Log::info = Log("INFO");
+const Log Log::debug = Log("DEBUG");
+std::function<void(const std::string &)> Log::writer{};
 
-        if (!address.empty() ) {
+// gran time in constructor
+Log::LogLine::LogLine(const Log & parent)
+  : std::basic_ios<char>{}, std::stringstream{""}
+{
+    time_t rawTime;
+    ::time(&rawTime);
 
-            if (!udp.bound)
-                udp.bind(5555);
+    std::string timeString = ctime(&rawTime);
 
-            udp.sendStr(address, buffer.str());
+    // remove newline
+    timeString.pop_back();
 
-        }
-    }
+    *this << "[" << parent.level_ << "]:" << timeString << " - ";
+}
 
-    string Log::time() {
-        std::string timeString;
-        time_t rawTime;
-        ::time(&rawTime);
-        timeString = ctime(&rawTime);
-        // remove newline
-        return timeString.substr(0, timeString.size() - 1);
-    }
+Log::LogLine::LogLine(LogLine &line)
+  : std::basic_ios<char>{}, std::stringstream{line.str()}
+{
+    // delete the original
+    line.str(std::string());
+}
 
-    Log Log::error() {
-        return Log("ERROR");
-    }
 
-    Log Log::warning() {
-        return Log("WARNING");
-    }
+Log::LogLine::~LogLine()
+{
+    // flush at destructor in the case it has not been flushed
+    if (str().empty())
+        return;
 
-    Log Log::info() {
-        return Log("INFO");
-    }
+    auto w = Log::writer;
+    if (w)
+        w(this->str());
+    str(std::string());
 
-    Log Log::debug() {
-        return Log("DEBUG");
-    }
+}
diff --git a/src/PIDController.cpp b/src/PIDController.cpp
index 783cbc4..0f39adb 100644
--- a/src/PIDController.cpp
+++ b/src/PIDController.cpp
@@ -2,10 +2,12 @@
 // Created by Matous Hybl on 2018-10-28.
 //
 
-#include <cmath>
-#include "roboutils/utils.h"
 #include "roboutils/PIDController.h"
-#include "roboutils/math.h"
+
+#include <cmath>
+#include "roboutils/util/swift.h"
+#include "roboutils/util/timing.h"
+#include "roboutils/util/mathematic.h"
 
 namespace RoboUtils {
 
@@ -37,4 +39,4 @@ namespace RoboUtils {
 
         return saturate(p * (error + integral + derivative), -absoluteMaximumAction, absoluteMaximumAction);
     }
-};
+}
diff --git a/src/Stopwatch.cpp b/src/Stopwatch.cpp
index 227c472..70b6dd8 100644
--- a/src/Stopwatch.cpp
+++ b/src/Stopwatch.cpp
@@ -1,7 +1,7 @@
 #include <iostream>
 #include <utility>
 #include "roboutils/Stopwatch.h"
-#include "roboutils/utils.h"
+#include "roboutils/util/timing.h"
 
 using namespace RoboUtils;
 using namespace std;
diff --git a/src/comm/Serial.cpp b/src/comm/Serial.cpp
new file mode 100644
index 0000000..6c8175e
--- /dev/null
+++ b/src/comm/Serial.cpp
@@ -0,0 +1,78 @@
+#include <roboutils/comm/Serial.h>
+#include "roboutils/util/strings.h"
+
+#include <stdexcept>
+#include <cerrno>
+#include <cstring>
+
+#include <termios.h>
+#include <unistd.h>
+#include <fcntl.h>
+
+using namespace RoboUtils::COMM;
+
+Serial::~Serial()
+{
+  if (fileDescriptor > 0) {
+    close(fileDescriptor);
+    fileDescriptor = 0;
+  }
+}
+
+void Serial::open(const std::string &file)
+{
+  fileDescriptor = ::open(file.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
+  if (fileDescriptor==-1)
+    throw std::logic_error("Can't open port");
+}
+
+void Serial::configure()
+{
+  struct termios tios{};
+
+  if (tcgetattr(fileDescriptor, &tios) < 0)
+    throw std::logic_error("can't get attr");
+
+  tios.c_cflag = CS8 | CLOCAL | CREAD;
+  tios.c_iflag = IGNPAR;
+  tios.c_oflag = 0;
+  tios.c_lflag = 0;
+  tios.c_cc[VMIN] = 1;      // block untill n bytes are received
+  tios.c_cc[VTIME] = 0;     // block untill a timer expires (n * 100 mSec.)
+
+  cfsetispeed(&tios, B115200);
+  cfsetospeed(&tios, B115200);
+
+  if (tcsetattr(fileDescriptor, TCSANOW, &tios) < 0)
+    throw std::logic_error("Error");
+}
+
+
+void Serial::send(const uint8_t *buffer, std::size_t size) const
+{
+  write(fileDescriptor, buffer, size);
+}
+
+
+std::vector<uint8_t> Serial::receive() const
+{
+  std::vector<uint8_t> result(1500);
+
+  int n = ::read(fileDescriptor, result.data(), (size_t)result.size());
+
+  if (n < 0) {
+    if (errno == EAGAIN)
+      return {};
+    else
+      throw std::logic_error("Can't read");
+  }
+
+  result.resize(n);
+  return result;
+}
+
+std::string Serial::receiveStr() const
+{
+  auto r = receive();
+  return { r.begin(), r.end() };
+}
\ No newline at end of file
diff --git a/src/comm/UDP.cpp b/src/comm/UDP.cpp
index 636e4d2..aeab505 100644
--- a/src/comm/UDP.cpp
+++ b/src/comm/UDP.cpp
@@ -3,7 +3,7 @@
 //
 
 #include <roboutils/comm/UDP.h>
-#include <roboutils/strings.h>
+#include "roboutils/util/strings.h"
 
 #include <algorithm>
 
@@ -88,7 +88,7 @@ void UDP::send(const std::string &host, const uint8_t *buffer, std::size_t size)
 
     SAHelper sa(host);
 
-    if (sendto(socketDescriptor, buffer, size, 0, (struct sockaddr *) &sa.sa, sizeof(sa.sa)) != size)
+    if (sendto(socketDescriptor, buffer, size, 0, (struct sockaddr *) &sa.sa, sizeof(sa.sa)) != (ssize_t) size)
         throw std::logic_error("Failed to send UDP packet  ");
 }
 
@@ -119,6 +119,9 @@ std::tuple<std::string, std::vector<uint8_t>> UDP::receive() const
     if (!bound)
         throw std::logic_error("UDP is not bound");
 
+    if (!available())
+        return {};
+
     SAHelper sa;
     unsigned int clientlen = sizeof(sa.sa);
     std::vector<uint8_t> result(1500);
diff --git a/src/io/ADC.cpp b/src/io/ADC.cpp
index a019451..13e0547 100644
--- a/src/io/ADC.cpp
+++ b/src/io/ADC.cpp
@@ -25,24 +25,85 @@ SOFTWARE.
 using namespace RoboUtils::IO;
 using namespace RoboUtils::Chips;
 
-ADC::ADC(I2C *i2c)
+ADC::ADC(const I2C &i2c)
+ : i2c_(i2c), chipAddress_(ADDR_AD799X_0_L)
 {
-    this->i2c = i2c;
-    chipAddress = ADDR_AD799X_0_L;
 }
 
-uint16_t ADC::readChannel(uint8_t channel)
+ADC::operator bool() const
 {
+    uint8_t reg;
+    return i2c_.read(chipAddress_, +Ad799X::Reg::CONFIG, &reg);
+}
+
+const I2C &ADC::bus() const
+{
+    return i2c_;
+}
+
+int ADC::chip() const
+{
+    return chipAddress_;
+}
+
+// mode 2
+std::map<int, uint16_t> ADC::Mode2Measure(int channel)
+{
+    uint16_t reg;
+    if (!i2c_.read(chipAddress_, Ad799X::RESULT::Reg(channel), &reg, Endian::Big))
+        throw adc_error();
+
+    return { {
+        Ad799X::RESULT::FromCHAN(reg),
+        Ad799X::RESULT::FromVALUE(reg) }
+    };
+}
+
+std::map<int, uint16_t> ADC::Mode2Measure(const std::vector<int>& channels)
+{
+    std::map<int, uint16_t> map;
+
+#if 1
+    uint16_t reg;
+
+    for (auto channel : channels) {
+
+        if (!i2c_.read(chipAddress_, Ad799X::RESULT::Reg(channel), &reg, Endian::Big))
+            throw adc_error();
+
+        map.insert({
+               Ad799X::RESULT::FromCHAN(reg),
+               Ad799X::RESULT::FromVALUE(reg)
+       });
+    }
+
+#else
+    // FIXME nasledujici kod by mel fungovat na dva i2c trnsacty dle DS ale nejede. Zrejme nepublikovana errata
+
+    std::vector<uint16_t> data(channels.size(), 0);
+    uint16_t cfg = 0;//Ad799X::CONFIG::FLTR;
 
-    uint16_t channelAndValue;
-    if (!i2c->read(chipAddress, Ad799X::RESULT::Reg(channel), &channelAndValue, false))
+    for (auto channel : channels)
+        cfg |= Ad799X::CONFIG::ToCh(channel);
+
+    if (!i2c_.write(chipAddress_, +Ad799X::Reg::CONFIG, cfg, false))
+        throw adc_error();
+
+    if (!i2c_.read(chipAddress_, +Ad799X::Reg::RESULT_SEQ, data.data(), (int)data.size(), false))
         throw adc_error();
 
-    uint16_t readChannel = Ad799X::RESULT::FromCHAN(channelAndValue);
+    for (auto i : data)
+        map.insert({
+            Ad799X::RESULT::FromCHAN(i),
+            Ad799X::RESULT::FromVALUE(i)
+        });
+#endif
 
-    // FIXME test if actually true
-    if (readChannel != channel)
-        throw std::logic_error("ADC: received result channel is not equal to requested channel");
+    return map;
+}
 
-    return Ad799X::RESULT::FromVALUE(channelAndValue );
+void ADC::setCycleMode(int divisor)
+{
+    if (!i2c_.write(chipAddress_, +Ad799X::Reg::CYCLE, (uint8_t)divisor, Endian::Big))
+        throw adc_error();
 }
\ No newline at end of file
diff --git a/src/io/GPIO.cpp b/src/io/GPIO.cpp
index 0a452f0..f1baefe 100644
--- a/src/io/GPIO.cpp
+++ b/src/io/GPIO.cpp
@@ -26,55 +26,75 @@ SOFTWARE.
 using namespace RoboUtils::IO;
 using namespace RoboUtils::Chips;
 
-GPIO::GPIO(I2C *i2c, int chipIndex)
+GPIO::GPIO(const I2C &i2c, int chipIndex)
+  : i2c_(i2c), chipAddress_(Mcp23017::Addr(chipIndex))
 {
-    this->i2c = i2c;
-    chipAddress = Mcp23017::Addr(chipIndex);
+}
+
+GPIO::operator bool() const
+{
+    uint8_t reg;
+    return i2c_.read(chipAddress_, +Mcp23017::Reg::IODIR, &reg);
+}
+
+const I2C &GPIO::bus() const
+{
+    return i2c_;
+}
+
+int GPIO::chip() const
+{
+    return chipAddress_;
+}
+
+void GPIO::set_chip(int address)
+{
+  chipAddress_ = address;
 }
 
 void GPIO::input(uint16_t pins, bool pullup) const
 {
-    if (!i2c->update<uint16_t>(chipAddress, +Mcp23017::Reg::IODIR, pins, 0, 0, true))
+    if (!i2c_.update<uint16_t>(chipAddress_, +Mcp23017::Reg::IODIR, pins, 0, 0,Endian::Little))
         throw gpio_error();
 
-    if (!i2c->update<uint16_t>(chipAddress, +Mcp23017::Reg::GPPU, pullup ? pins : 0, pullup ? 0 : pins, 0, true))
+    if (!i2c_.update<uint16_t>(chipAddress_, +Mcp23017::Reg::GPPU, pullup ? pins : 0, pullup ? 0 : pins, 0,Endian::Little))
         throw gpio_error();
 }
 
 void GPIO::output(uint16_t pins) const
 {
-    if (!i2c->update<uint16_t>(chipAddress, +Mcp23017::Reg::IODIR, 0, pins, 0, true))
+    if (!i2c_.update<uint16_t>(chipAddress_, +Mcp23017::Reg::IODIR, 0, pins, 0,Endian::Little))
         throw gpio_error();
 }
 
 void GPIO::set(uint16_t pins, bool value) const
 {
-    if (!i2c->update<uint16_t>(chipAddress,  +Mcp23017::Reg::OLAT, value ? 0 : pins, value ? pins : 0, 0, true))
+    if (!i2c_.update<uint16_t>(chipAddress_,  +Mcp23017::Reg::OLAT, value ? 0 : pins, value ? pins : 0, 0, Endian::Little))
         throw gpio_error();
 }
 
 void GPIO::low(uint16_t pins) const
 {
-    if (!i2c->update<uint16_t>(chipAddress,  +Mcp23017::Reg::OLAT, pins, 0, 0, true))
+    if (!i2c_.update<uint16_t>(chipAddress_,  +Mcp23017::Reg::OLAT, pins, 0, 0, Endian::Little))
         throw gpio_error();
 }
 
 void GPIO::high(uint16_t pins) const
 {
-    if (!i2c->update<uint16_t>(chipAddress,  +Mcp23017::Reg::OLAT, 0, pins, 0, true))
+    if (!i2c_.update<uint16_t>(chipAddress_,  +Mcp23017::Reg::OLAT, 0, pins, 0, Endian::Little))
         throw gpio_error();
 }
 
 void GPIO::toggle(uint16_t pins) const
 {
-    if (!i2c->update<uint16_t>(chipAddress,  +Mcp23017::Reg::OLAT, 0, 0, pins, true))
+    if (!i2c_.update<uint16_t>(chipAddress_,  +Mcp23017::Reg::OLAT, 0, 0, pins, Endian::Little))
         throw gpio_error();
 }
 
 bool GPIO::get(uint16_t pins) const
 {
     uint16_t p = 0;
-    if (!i2c->read(chipAddress, +Mcp23017::Reg::GPIO, &p, true))
+    if (!i2c_.read(chipAddress_, +Mcp23017::Reg::GPIO, &p, Endian::Little))
         throw gpio_error();
 
     return (pins & p) != 0;
@@ -83,7 +103,7 @@ bool GPIO::get(uint16_t pins) const
 uint16_t GPIO::read(uint16_t pins) const
 {
     uint16_t p = 0;
-    if (!i2c->read(chipAddress, +Mcp23017::Reg::GPIO, &p, true))
+    if (!i2c_.read(chipAddress_, +Mcp23017::Reg::GPIO, &p, Endian::Little))
         throw gpio_error();
 
     return p & pins;
diff --git a/src/io/I2C.cpp b/src/io/I2C.cpp
index e9b6991..987b4cb 100644
--- a/src/io/I2C.cpp
+++ b/src/io/I2C.cpp
@@ -22,62 +22,87 @@ SOFTWARE.
 
 #include <linux/i2c-dev.h>
 #include <sys/ioctl.h> // ioctl
+#include <linux/i2c.h>
 #include <fcntl.h> // O_RDWR
 #include <unistd.h>  // open, close
 
+#include <vector>
+
 #if !defined(__linux__)
 # error "This part of library is not compatible with your setup"
 #endif
 
 using namespace RoboUtils::IO;
 
-I2C::I2C(const std::string &busFile)
+
+I2C::~I2C()
 {
-    // FIXME: Wrong design, breaking constructor rule - only memory allocation allowed, no
-    i2cDescriptor = open(busFile.c_str(), O_RDWR);
+    if (i2cDescriptor > 0) {
+        close(i2cDescriptor);
+        i2cDescriptor = 0;
+    }
 }
 
-I2C::~I2C()
+I2C::operator bool() const
+{
+    return i2cDescriptor > 0;
+}
+
+bool I2C::open(const std::string &busFile)
 {
-    // FIXME: Do not close when wrongly opened
-    close(i2cDescriptor);
+    if (i2cDescriptor > 0) {
+        close(i2cDescriptor);
+        i2cDescriptor = 0;
+    }
+
+    bus_ = busFile;
+    if (bus_.empty())
+        return false;
+
+    i2cDescriptor = ::open(bus_.c_str(), O_RDWR);
+    return i2cDescriptor > 0;
+}
+
+std::string I2C::bus() const
+{
+    return bus_;
 }
 
 template <typename T>
-bool I2C::write(const int chipAddress, const int registerAddress, const T value, bool littleEndian) const
+bool I2C::write(const int chipAddress, const int registerAddress, const T value, Endian endian) const
 {
-    return write_(chipAddress, registerAddress, reinterpret_cast<const uint8_t *>(&value), sizeof(T), 1, littleEndian);
+    return write_(chipAddress, registerAddress, reinterpret_cast<const uint8_t *>(&value), sizeof(T), 1, endian);
 }
 
 template <typename T>
-bool I2C::write(const int chipAddress, const int registerAddress, const T* array, const int count, bool littleEndian) const
+bool I2C::write(const int chipAddress, const int registerAddress, const T* array, const int count, Endian endian) const
 {
-    return write_(chipAddress, registerAddress, reinterpret_cast<const uint8_t *>(array), sizeof(T), count, littleEndian);
+    return write_(chipAddress, registerAddress, reinterpret_cast<const uint8_t *>(array), sizeof(T), count, endian);
 }
 
 template <typename T>
-bool I2C::read(const int chipAddress, const int registerAddress, T* value, bool littleEndian) const
+bool I2C::read(const int chipAddress, const int registerAddress, T* value, Endian endian) const
 {
-    return read_(chipAddress, registerAddress, reinterpret_cast<uint8_t *>(value), sizeof(T), 1, littleEndian);
+    return read_(chipAddress, registerAddress, reinterpret_cast<uint8_t *>(value), sizeof(T), 1, endian);
 }
 
 template <typename T>
-bool I2C::read(const int chipAddress, const int registerAddress, T* array, const int count, bool littleEndian) const
+bool I2C::read(const int chipAddress, const int registerAddress, T* array, const int count, Endian endian) const
 {
-    return read_(chipAddress, registerAddress, reinterpret_cast<uint8_t *>(array), sizeof(T), count, littleEndian);
+    return read_(chipAddress, registerAddress, reinterpret_cast<uint8_t *>(array), sizeof(T), count, endian);
 }
 
 template<typename T>
-bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, T setBits, T clearBits, T toggleBits, bool littleEncian) const
+bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, T setBits, T clearBits, T toggleBits, Endian endian) const
 {
     T value;
 
-    if (!read(chipAddress, registerAddress, &value, littleEncian))
+    if (!read(chipAddress, registerAddress, &value, endian))
         return false;
 
     value = ((value & ~clearBits) | setBits) ^ toggleBits;
 
-    return write(chipAddress, registerAddress, value, littleEncian);
+    return write(chipAddress, registerAddress, value, endian);
 }
 
 
@@ -86,61 +111,50 @@ bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, T setBits, T clea
 // PRIVATE
 
 //#define LOG // TODO log later using matous's engine
-bool I2C::transact_(int addr, uint8_t *w, uint32_t wn, uint8_t *r, uint32_t rn) const
+bool I2C::transact_(int addr, uint8_t *w, int wn, uint8_t *r, int rn) const
 {
-    std::lock_guard <std::mutex> lock(mutex);
-
-    if (i2cDescriptor < 0)
-        throw i2c_error();
-
-#ifdef LOG
-    printf( "I2C: @%02x TRANSN W %d R %d ", addr, wn, rn);
-    if (wn > 0) {
-        printf("[ ");
-        for (int i = 0 ; i < wn ; ++i)
-            printf("%02x ", w[i]);
-        printf("] ");
-    }
-#endif
+  std::lock_guard <std::mutex> lock(mutex);
 
-    if (ioctl(i2cDescriptor, I2C_SLAVE, addr))
-        throw i2c_error();
+  if (i2cDescriptor < 0)
+    throw i2c_error();
 
-    if (wn > 0) {
-        int err = ::write(i2cDescriptor, w, wn);
+  if (wn && rn) {
 
+    i2c_msg messages[2] = {
+            {static_cast<uint16_t>(addr), 0,        static_cast<uint16_t>(wn), w},
+            {static_cast<uint16_t>(addr), I2C_M_RD, static_cast<uint16_t>(rn), r}
+    };
 
-#ifdef LOG
-        printf("(%d written) ", err);
-#endif
-        if (err != wn)
-            return false;
-    }
+    i2c_rdwr_ioctl_data command = {messages, 2};
+    return ::ioctl(i2cDescriptor, I2C_RDWR, &command) > 0;
 
-    if (rn > 0) {
-        int err = ::read(i2cDescriptor, r, rn);
+  } else if (wn) {
+    i2c_msg messages[1] = {
+            {static_cast<uint16_t>(addr), 0,        static_cast<uint16_t>(wn), w},
+    };
 
-#ifdef LOG
-        printf("(%d readed) [ ", err);
-        for (int i = 0 ; i < rn ; ++i)
-            printf("%2x ", r[i]);
-        printf("]\n");
-#endif
-        return err == rn;
-    }
+    i2c_rdwr_ioctl_data command = {messages, 1};
+    return ::ioctl(i2cDescriptor, I2C_RDWR, &command) > 0;
 
-#ifdef LOG
-    printf("\n");
-#endif
-    return true;
+  } else if (rn) {
+
+    i2c_msg messages[1] = {
+            {static_cast<uint16_t>(addr), I2C_M_RD, static_cast<uint16_t>(rn), r}
+    };
+
+    i2c_rdwr_ioctl_data command = {messages, 1};
+    return ::ioctl(i2cDescriptor, I2C_RDWR, &command) > 0;
+  }
+
+  return false;
 }
 
-bool I2C::write_(int chipAddress, int registerAddress,  const uint8_t *data, int typeSize, int size, bool litleEndian  ) const
+bool I2C::write_(int chipAddress, int registerAddress,  const uint8_t *data, int typeSize, int size, Endian endian) const
 {
-    uint8_t buff[1 + size * typeSize];
+    std::vector<uint8_t> buff(1+size * typeSize);
     buff[0] = registerAddress;
 
-    if (litleEndian) {
+    if (endian == Endian::Little) {
 
         for (int i = 0 ; i < size * typeSize ; ++i)
             buff[i+1] = data[i];
@@ -154,19 +168,20 @@ bool I2C::write_(int chipAddress, int registerAddress,  const uint8_t *data, int
 
     }
 
-    return transact_(chipAddress, buff, 1 + size * typeSize, nullptr, 0);
+    return transact_(chipAddress, buff.data(), (int)buff.size(), nullptr, 0);
 }
 
 
-bool I2C::read_(int chipAddress, int registerAddress,  uint8_t *data, int typeSize, int size, bool litleEndian  ) const
+bool I2C::read_(int chipAddress, int registerAddress,  uint8_t *data, int typeSize, int size, Endian endian) const
 {
-    uint8_t buff[size * typeSize];
+    std::vector<uint8_t> buff(size * typeSize);
+
     buff[0] = registerAddress;
 
-    if (!transact_(chipAddress, buff, 1 , buff, size * typeSize))
+    if (!transact_(chipAddress, buff.data(), 1 , buff.data(), (int)buff.size()))
         return false;
 
-    if (litleEndian) {
+    if (endian == Endian::Little) {
 
         for (int i = 0; i < size * typeSize; ++i)
             data[i] = buff[i];
@@ -186,64 +201,64 @@ bool I2C::read_(int chipAddress, int registerAddress,  uint8_t *data, int typeSi
 //----------------------------------------------------------------------------------------------------------------------
 // INSTANTIATION
 
-template bool I2C::write(int chipAddress, int registerAddress, uint8_t val, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, uint16_t val, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, uint32_t val, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, uint64_t val, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, int8_t val, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, int16_t val, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, int32_t val, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, int64_t val, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, float val, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, double val, bool littleEndian) const;
+template bool I2C::write(int chipAddress, int registerAddress, uint8_t val, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, uint16_t val, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, uint32_t val, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, uint64_t val, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, int8_t val, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, int16_t val, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, int32_t val, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, int64_t val, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, float val, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, double val, Endian endian) const;
 
 //----------------------------------------------------------------------------------------------------------------------
 
-template bool I2C::write(int chipAddress, int registerAddress, const uint8_t *val, const int count, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, const uint16_t *val, const int count, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, const uint32_t *val, const int count, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, const uint64_t *val, const int count, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, const int8_t *val, const int count, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, const int16_t *val, const int count, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, const int32_t *val, const int count, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, const int64_t *val, const int count, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, const float *val, const int count, bool littleEndian) const;
-template bool I2C::write(int chipAddress, int registerAddress, const double *val, const int count, bool littleEndian) const;
+template bool I2C::write(int chipAddress, int registerAddress, const uint8_t *val, const int count, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, const uint16_t *val, const int count, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, const uint32_t *val, const int count, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, const uint64_t *val, const int count, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, const int8_t *val, const int count, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, const int16_t *val, const int count, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, const int32_t *val, const int count, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, const int64_t *val, const int count, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, const float *val, const int count, Endian endian) const;
+template bool I2C::write(int chipAddress, int registerAddress, const double *val, const int count, Endian endian) const;
 
 //----------------------------------------------------------------------------------------------------------------------
 
-template bool I2C::read(int chipAddress, int registerAddress, uint8_t* val, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, uint16_t* val, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, uint32_t* val, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, uint64_t* val, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, int8_t* val, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, int16_t* val, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, int32_t* val, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, int64_t* val, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, float* val, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, double* val, bool littleEndian) const;
+template bool I2C::read(int chipAddress, int registerAddress, uint8_t* val, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, uint16_t* val, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, uint32_t* val, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, uint64_t* val, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, int8_t* val, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, int16_t* val, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, int32_t* val, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, int64_t* val, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, float* val, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, double* val, Endian endian) const;
 
 //----------------------------------------------------------------------------------------------------------------------
 
-template bool I2C::read(int chipAddress, int registerAddress, uint8_t* array, const int count, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, uint16_t* array, const int count, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, uint32_t* array, const int count, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, uint64_t* array, const int count, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, int8_t* array, const int count, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, int16_t* array, const int count, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, int32_t* array, const int count, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, int64_t* array, const int count, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, float* array, const int count, bool littleEndian) const;
-template bool I2C::read(int chipAddress, int registerAddress, double* array, const int count, bool littleEndian) const;
+template bool I2C::read(int chipAddress, int registerAddress, uint8_t* array, const int count, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, uint16_t* array, const int count, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, uint32_t* array, const int count, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, uint64_t* array, const int count, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, int8_t* array, const int count, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, int16_t* array, const int count, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, int32_t* array, const int count, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, int64_t* array, const int count, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, float* array, const int count, Endian endian) const;
+template bool I2C::read(int chipAddress, int registerAddress, double* array, const int count, Endian endian) const;
 
 //----------------------------------------------------------------------------------------------------------------------
 
-template bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, uint8_t setBits, uint8_t clearBits, uint8_t toggleBits, bool littleEndian) const;
-template bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, uint16_t setBits, uint16_t clearBits, uint16_t toggleBits, bool littleEndian) const;
-template bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, uint32_t setBits, uint32_t clearBits, uint32_t toggleBits, bool littleEndian) const;
-template bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, uint64_t setBits, uint64_t clearBits, uint64_t toggleBits, bool littleEndian) const;
-template bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, int8_t setBits, int8_t clearBits, int8_t toggleBits, bool littleEndian) const;
-template bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, int16_t setBits, int16_t clearBits, int16_t toggleBits, bool littleEndian) const;
-template bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, int32_t setBits, int32_t clearBits, int32_t toggleBits, bool littleEndian) const;
-template bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, int64_t setBits, int64_t clearBits, int64_t toggleBits, bool littleEndian) const;
+template bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, uint8_t setBits, uint8_t clearBits, uint8_t toggleBits, Endian endian) const;
+template bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, uint16_t setBits, uint16_t clearBits, uint16_t toggleBits, Endian endian) const;
+template bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, uint32_t setBits, uint32_t clearBits, uint32_t toggleBits, Endian endian) const;
+template bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, uint64_t setBits, uint64_t clearBits, uint64_t toggleBits, Endian endian) const;
+template bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, int8_t setBits, int8_t clearBits, int8_t toggleBits, Endian endian) const;
+template bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, int16_t setBits, int16_t clearBits, int16_t toggleBits, Endian endian) const;
+template bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, int32_t setBits, int32_t clearBits, int32_t toggleBits, Endian endian) const;
+template bool I2C::update(uint8_t chipAddress, uint8_t registerAddress, int64_t setBits, int64_t clearBits, int64_t toggleBits, Endian endian) const;
 // float a double nedavaji smysl
diff --git a/src/io/KM2.cpp b/src/io/KM2.cpp
index f7b1d0a..f4ae2e8 100644
--- a/src/io/KM2.cpp
+++ b/src/io/KM2.cpp
@@ -19,22 +19,37 @@ SOFTWARE.
 
 #include <roboutils/io/KM2.h>
 #include <roboutils/chips/km2chip.h>
-#include <roboutils/utils.h>
+#include "roboutils/util/timing.h"
 
 using namespace RoboUtils::IO;
 using namespace RoboUtils::Chips;
 
-KM2::KM2(I2C *aBus, int aChipAddress)
+KM2::KM2(const I2C &aBus, int aChipAddress)
+ : bus_(aBus), chipAddress_(aChipAddress)
 {
-    bus = aBus;
-    chipAddress = aChipAddress;
+}
+
+KM2::operator bool() const
+{
+    uint8_t reg;
+    return bus_.read(chipAddress_, +Km2::Reg::SPEED, &reg);
+}
+
+const I2C &KM2::bus() const
+{
+    return bus_;
+}
+
+int KM2::chip() const
+{
+    return chipAddress_;
 }
 
 void KM2::drive(int left, int right) const
 {
     int16_t speed[2] {(int16_t)left,(int16_t)right};
 
-    if (!bus->write(chipAddress, +Km2::Reg::SPEED, speed, 2, true))
+    if (!bus_.write(chipAddress_, +Km2::Reg::SPEED, speed, 2, Endian::Little))
         throw km2_error();
 }
 
@@ -42,7 +57,7 @@ std::tuple<int,int> KM2::odometry() const
 {
     int32_t result[2] {0 , 0};
 
-    if (!bus->read(chipAddress, +Km2::Reg::ODOMETRY, result, 2, true))
+    if (!bus_.read(chipAddress_, +Km2::Reg::ODOMETRY, result, 2, Endian::Little))
         throw km2_error();
 
     return std::tie(result[0], result[1]);
@@ -61,6 +76,6 @@ void KM2::setAddress(int newaddr, bool bcast) const
     if (bcast)
         addr |= Km2::CFGADDR::BCASTEN;
 
-    if (!bus->write(chipAddress, +Km2::Reg::CFGADDR, addr, true))
+    if (!bus_.write(chipAddress_, +Km2::Reg::CFGADDR, addr, Endian::Little))
         throw km2_error();
 }
\ No newline at end of file
diff --git a/src/strings.cpp b/src/strings.cpp
deleted file mode 100644
index ecefc6c..0000000
--- a/src/strings.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-#include <roboutils/strings.h>
-
-#include <sstream>
-
-using namespace RoboUtils;
-
-std::vector<std::string> RoboUtils::split(const std::string& text, char delim) {
-    std::string line;
-    std::vector<std::string> vec;
-    std::stringstream ss(text);
-
-    while (std::getline(ss, line, delim))
-        vec.push_back(line);
-
-    return vec;
-}
-
-std::string RoboUtils::join(const std::vector<std::string>& data, const std::string& delim)
-{
-    std::string result;
-    std::string d;
-
-    result.reserve(data.size() * 12); // 12 chars per single data on average
-
-    for (auto i : data) {
-        result.append(d);
-        result.append(i);
-        d = delim;
-    }
-
-    return result;
-}
diff --git a/src/threading.cpp b/src/threading.cpp
deleted file mode 100644
index 67eac4a..0000000
--- a/src/threading.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-//
-// Created by Matous Hybl on 2019-02-11.
-//
-
-#include <thread>
-#include "roboutils/threading.h"
-#include "roboutils/utils.h"
-
-namespace RoboUtils {
-    void repeatAsynchronously(unsigned int periodMs, const std::function<void(void)>& fun) {
-        std::thread([periodMs, fun]() {
-#pragma clang diagnostic push
-#pragma clang diagnostic ignored "-Wmissing-noreturn"
-            while (true) {
-                let start = millis();
-                fun();
-                let delayTime = periodMs - (millis() - start);
-                if (delayTime > 0) {
-                    std::this_thread::sleep_for(std::chrono::milliseconds(periodMs));
-                }
-            }
-#pragma clang diagnostic pop
-        }).detach();
-    }
-};
\ No newline at end of file
diff --git a/src/util/locales.cpp b/src/util/locales.cpp
new file mode 100644
index 0000000..489d0d0
--- /dev/null
+++ b/src/util/locales.cpp
@@ -0,0 +1,20 @@
+#include "roboutils/util/locales.h"
+
+using namespace RoboUtils;
+
+template <int LC>
+Locale<LC>::Locale(const char *loc)
+ : loc_{std::setlocale(LC, nullptr)}
+{
+  std::setlocale(LC, loc);
+}
+
+template <int LC>
+Locale<LC>::~Locale()
+{
+  std::setlocale(LC,loc_.c_str());
+}
+
+// Explicit instantiation
+template class RoboUtils::Locale<LC_NUMERIC>;
+template class RoboUtils::Locale<LC_TIME>;
\ No newline at end of file
diff --git a/src/util/strings.cpp b/src/util/strings.cpp
new file mode 100644
index 0000000..d77229b
--- /dev/null
+++ b/src/util/strings.cpp
@@ -0,0 +1,76 @@
+#include "roboutils/util/strings.h"
+
+#include <sstream>
+#include <cmath>
+#include <iomanip>
+
+using namespace RoboUtils;
+
+std::vector<std::string> RoboUtils::split(const std::string& text, char delim)
+{
+    std::string line;
+    std::vector<std::string> vec;
+    std::stringstream ss(text);
+
+    while (std::getline(ss, line, delim))
+        vec.push_back(line);
+
+    return vec;
+}
+
+std::string RoboUtils::join(const std::vector<std::string>& data, const std::string& delim)
+{
+    std::string result;
+    std::string d;
+
+    result.reserve(data.size() * 12); // 12 chars per single data on average
+
+    for (const auto &i : data) {
+        result.append(d);
+        result.append(i);
+        d = delim;
+    }
+
+    return result;
+}
+
+std::string RoboUtils::join(const std::vector<std::string>::iterator & begin, const std::vector<std::string>::iterator & end, const std::string& delim)
+{
+  std::vector<std::string>::iterator i = begin;
+
+  if (i == end)
+    return {};
+
+  std::string result{*i++};
+
+  while (i != end) {
+    result.append(delim);
+    result.append(*i++);
+  }
+
+  return result;
+}
+
+std::string RoboUtils::to_fixed(double d, int decimals)
+{
+  if (std::isnan(d))
+    return {};
+
+  std::ostringstream results;
+  results << std::fixed << std::setprecision(decimals) << d;
+  return results.str();
+}
+
+void RoboUtils::eraseBefore(std::string &s, char c)
+{
+  auto pos = s.find(c);
+  if (pos > 0)
+    s.erase(0, pos);
+}
+
+void RoboUtils::eraseBefore(std::string &s, const std::string &c)
+{
+  auto pos = s.find(c);
+  if (pos > 0)
+    s.erase(0, pos);
+}
\ No newline at end of file
diff --git a/src/util/threading.cpp b/src/util/threading.cpp
new file mode 100644
index 0000000..bc84cbe
--- /dev/null
+++ b/src/util/threading.cpp
@@ -0,0 +1,23 @@
+//
+// Created by Matous Hybl on 2019-02-11.
+//
+
+#include <thread>
+#include "roboutils/util/threading.h"
+#include "roboutils/util/timing.h"
+#include "roboutils/util/swift.h"
+
+void RoboUtils::repeatAsynchronously(unsigned int periodMs, const std::function<bool(void)>& fun)
+{
+    std::thread([periodMs, fun]() {
+        bool ret = true;
+        while (ret) {
+            let start = millis();
+            ret = fun();
+            let delayTime = periodMs - (millis() - start);
+            if (delayTime > 0) {
+                std::this_thread::sleep_for(std::chrono::milliseconds(periodMs));
+            }
+        }
+    }).detach();
+}
\ No newline at end of file
diff --git a/src/util/timing.cpp b/src/util/timing.cpp
new file mode 100644
index 0000000..1bc770d
--- /dev/null
+++ b/src/util/timing.cpp
@@ -0,0 +1,35 @@
+//
+// Created by Matous Hybl on 2018-11-14.
+//
+
+#include "roboutils/util/timing.h"
+
+#include <chrono>
+#include <unistd.h>
+
+long long RoboUtils::millis()
+{
+    return std::chrono::duration_cast<std::chrono::milliseconds>(
+            std::chrono::system_clock::now().time_since_epoch()
+    ).count();
+}
+
+void RoboUtils::delay(long ms)
+{
+    usleep(ms * 1000);
+}
+
+bool RoboUtils::expired(long long *last_event, long delay, long long ms)
+{
+    if (!last_event)
+        return false;
+
+    if (ms < 0)
+        ms = millis();
+
+    if (ms < (*last_event + delay))
+        return false;
+
+    *last_event = ms;
+    return true;
+}
\ No newline at end of file
diff --git a/src/Variable.cpp b/src/util/variable.cpp
similarity index 56%
rename from src/Variable.cpp
rename to src/util/variable.cpp
index 85b756a..e19093e 100644
--- a/src/Variable.cpp
+++ b/src/util/variable.cpp
@@ -2,4 +2,4 @@
 // Created by Matous Hybl on 2018-11-14.
 //
 
-#include "roboutils/Variable.h"
+#include "roboutils/util/variable.h"