diff --git a/src/gy521_driver/gy521_driver.c b/src/gy521_driver/gy521_driver.c
index 2bd095c..e705c25 100644
--- a/src/gy521_driver/gy521_driver.c
+++ b/src/gy521_driver/gy521_driver.c
@@ -6,6 +6,36 @@
 #include "gy521_driver.h"
 #include <stdint.h>
 
+/*Stuff for cmocka*/
+#if UNIT_TESTING
+extern void* _test_malloc(const size_t size, const char* file, const int line);
+extern void* _test_calloc(const size_t number_of_elements, const size_t size,
+                          const char* file, const int line);
+extern void _test_free(void* const ptr, const char* file, const int line);
+
+#define malloc(size) _test_malloc(size, __FILE__, __LINE__)
+#define calloc(num, size) _test_calloc(num, size, __FILE__, __LINE__)
+#define free(ptr) _test_free(ptr, __FILE__, __LINE__)
+#endif // UNIT_TESTING
+
+/*
+ * ############################
+ * REGISTER MAP 
+ * ############################
+ */ 
+
+enum gy521_map {
+    fifo_couth = 0x72,
+    fifo_contl,
+    fifo_r_w,
+    who_am_i,
+};
+
+/*
+ * ############################
+ * Structures 
+ * ############################
+ */ 
 
 typedef struct{
     uint16_t x;
@@ -19,35 +49,60 @@ typedef struct{
     uint16_t z;
 }accel_values_struct;
 
-typedef struct{
-    _Bool (*init)(uint8_t slv_addr); 
-}methods_struct;
-
-typedef struct{
+struct gy521_module{
     uint8_t slave_address;
     gyro_values_struct gyro;
     accel_values_struct accel;
-    methods_struct methods;
-}gy521_module_struct;
+};
+
+/*
+ * ############################
+ * Function Definitions
+ * ############################
+ */
+
+gy521_module* gy521_new(void)
+{
+    //gy521_module *m = malloc(sizeof(gy521_module));
+    gy521_module *m = (gy521_module *)malloc(sizeof(gy521_module));
+    m->slave_address = 0x0;
+    return m;
+}
 
 
-_Bool init_gy521(uint8_t slave_address) {
-    if(slave_address != TWI_GY521_ADDR1 && slave_address != TWI_GY521_ADDR2) {
+void gy521_free(gy521_module *m)
+{
+    free(m);
+}
+
+_Bool gy521_init(gy521_module *m, uint8_t slave_address) {
+    if((slave_address != TWI_GY521_ADDR1) && (slave_address != TWI_GY521_ADDR2)) { 
+        m->slave_address = 0x0;
+        return 0;
+    }
+
+    /*Set the slave address of the module*/
+    m->slave_address = slave_address;
+
+    /*Try  to talk with i2c module.*/
+    uint8_t data = who_am_i;
+    gy521_twi_tx(m->slave_address, &data, 1);
+    gy521_twi_rx(m->slave_address, &data, 1);
+    
+    if(data != m->slave_address) {
         return 0;
     }
 
     return 1;
 }
 
-_Bool module_test(gy521_module_struct *m)
+
+_Bool gy521_update_gyro(gy521_module *m)
 {
-    _Bool ret = m->methods.init(TWI_GY521_ADDR1);
-    return ret;
+    return 0;
 }
 
-
-/* returns 1 */  
-uint8_t test_function(void)
+_Bool gy521_update_accel(gy521_module *m)
 {
-    return 1;
+    return 0;
 }
diff --git a/src/gy521_driver/gy521_driver.h b/src/gy521_driver/gy521_driver.h
index fde1227..64b0d1c 100644
--- a/src/gy521_driver/gy521_driver.h
+++ b/src/gy521_driver/gy521_driver.h
@@ -8,6 +8,7 @@
 #define GY521_DRIVER_H
 
 #include <stdint.h>
+#include <stdlib.h>
 
 /*
  * ############################
@@ -24,8 +25,7 @@
  * Types/Structures 
  * ############################
  */ 
-//typedef struct gy521_module_struct *gy521_module;
-typedef struct gy521_module_struct *gy521_module;
+typedef struct gy521_module gy521_module;
 
 /*
  * ############################
@@ -33,17 +33,18 @@ typedef struct gy521_module_struct *gy521_module;
  * ############################
  */ 
 
+
 /*Function pointers for the TX and RX fuctionality*/
-extern void twi_tx(uint8_t *data, uint8_t size);
-extern void twi_rx(uint8_t *data, uint8_t size);
 
-_Bool init_gy521(uint8_t slave_address);
+extern void (*gy521_twi_tx)(uint8_t, uint8_t*, uint8_t);
+extern void (*gy521_twi_rx)(uint8_t, uint8_t*, uint8_t);
 
 
-
-_Bool module_test(struct gy521_module_struct *m);
-uint8_t test_function(void);
-
+struct gy521_module* gy521_new(void);
+_Bool gy521_init(struct gy521_module *m, uint8_t slave_address);
+_Bool gy521_update_gyro(struct gy521_module* m);
+_Bool gy521_update_accel(struct gy521_module* m);
+void gy521_free(struct gy521_module *m);
 
 
 #endif /* GY521_DRIVER_H */