diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_config.cpp b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_config.cpp
index 2703ca5..7c74583 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_config.cpp
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_config.cpp
@@ -1,5 +1,3 @@
-//#pragma once
-//#include "pP_function.h"
 #include "pP_config.h"
 #include <EEPROM.h>
 
diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c.cpp b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c.cpp
index 057449d..fc23912 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c.cpp
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c.cpp
@@ -5,23 +5,59 @@
 #include "pP_i2c.h"
 #include <Wire.h>
 
+byte registerMap[regMapSize];
+byte registerMapTemp[regMapSize - 1];
+byte receivedCommands[maxBytes];
+
 pP_i2c::pP_i2c(){
 
 }
 
 void pP_i2c::init() {
   Wire.begin(pP_i2c_address);
+  Wire.onRequest(i2cReply);
+  Wire.onReceive(i2cInput);
+}
+
+void pP_i2c::i2cReportStatus() {
+  _i2cResponse = "{"
+}
+
+void pP_i2c::i2cReportVersion() {
+
+}
+
+void pP_i2c::i2cReportConfig() {
+
+}
+
+void pP_i2c::i2cReportIdentity() {
+
+}
+
+void pP_i2c::i2cRequestInput() {
+
+}
+
+void pP_i2c::i2cReply() {
+  Wire.send()
 }
 
 void pP_i2c::i2cInput(int bytesReceived) {
   for (int a = 0; a < bytesReceived; a++) {
-    if (a < maxBytes) {
-      cmdRcvd[a] = Wire.read();
+    // Check length of message, drops anything longer than [longBytes]
+    if (a <= maxBytes) {
+      cmdRcvd[a] = Wire.receive();
+    }
+    elif (a <= longBytes) {
+      longRcvd[a] = Wire.receive();
     }
     else {
-      longRcvd[a] = Wire.read();
+      Wire.receive(); //
     }
   }
+
+  // Check input command corresponds with register map, set 0x00 if not
   if (bytesReceived == 1 && (cmdRcvd[0] < regMapSize)) {
     return;
   }
@@ -29,35 +65,65 @@ void pP_i2c::i2cInput(int bytesReceived) {
     cmdRcvd[0] = 0x00;
     return;
   }
+
+  // Parse commands and apply changes or actions
   switch (cmdRcvd[0]) {
     case 0x00:
-      followerInt = (long) cmdRcvd[1];
+      i2cReportStatus();
       return;
       break;
     case 0x01:
-      compInt = (long) cmdRcvd[1];
+      followerInt = (int) cmdRcvd[1];
       return;
       break;
     case 0x02:
-      GAIN_FACTOR = (uint8_t) cmdRcvd[1];
+      compInt = (int) cmdRcvd[1];
       return;
       break;
     case 0x03:
-      Hyst = (uint8_t) cmdRcvd[1];
+      GAIN_FACTOR = (int) cmdRcvd[1];
       return;
       break;
     case 0x04:
-      LOOP_DUR = (uint8_t) cmdRcvd[1];
+      Hyst = (int) cmdRcvd[1];
       return;
       break;
     case 0x05:
-      TRG_DUR = (uint8_t) cmdRcvd[1];
+      LOOP_DUR = (int) cmdRcvd[1];
       return;
       break;
     case 0x06:
+      LOGIC = (int) cmdRcvd[1];
+      return;
+      break;
+    case 0x07:
+      PZDET = (int) cmdRcvd[1];
+      return;
+      break;
+    case 0x08:
+      TRG_DUR = (int) cmdRcvd[1];
+      return;
+      break;
+    case 0x09:
+      DEBUG = (int) cmdRcvd[1];
+      return;
+      break;
+    case 0x0a:
       voltMeterConstant = longRcvd[0]*65536+longRcvd[1]*256+longRcvd[2];
       return;
       break;
+    case 0x0b:
+      reportVersion();
+      return;
+      break;
+    case 0x0c:
+      reportConfig();
+      return;
+      break;
+    case 0x0d:
+      reportIdentity();
+      return;
+      break;
     default:
       return;
   }
diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c.h b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c.h
index 6e49724..a21df47 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c.h
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c.h
@@ -2,18 +2,24 @@
 #define _pP_i2c_h_
 #ifdef I2C_INPUT
 
-
-#define senseInt_Offset 0x00 // Integer of sense threshold in millivolts
-#define compInt_Offset  0x01 // Integer of comparator threshold in millivolts
-#define gainFactor_Offset 0x02  // Gain adjustment factor. 0=3x, 1=3.5x, 2=4.33x, 3=6x, 4=11x
-#define hysteresis_Offset 0x03  // Hysteresis value for ADC measurements
-#define loopDuration_Offset 0x04  // duration of time between ADC checks and other loop functions
-#define triggerDuration_Offset  0x05 // duration of the Z-axis pulse sent, in ms
-#define voltMeterLong_Offset 0x06  // For fine-tuning the input volt master
+#define status_Offset 0x00 // Status register
+#define senseInt_Offset 0x01 // Integer of sense threshold in millivolts
+#define compInt_Offset  0x02 // Integer of comparator threshold in millivolts
+#define gainFactor_Offset 0x03  // Gain adjustment factor. 0=3x, 1=3.5x, 2=4.33x, 3=6x, 4=11x
+#define hysteresis_Offset 0x04  // Hysteresis value for ADC measurements
+#define loopDuration_Offset 0x05  // duration of time between ADC checks and other loop functions
+#define logicLevel_Offset 0x06
+#define piezoDetect_Offset 0x07
+#define triggerDuration_Offset  0x08 // duration of the Z-axis pulse sent, in ms
+#define debugEnable_Offset 0x09
+#define voltMeterLong_Offset 0x0a  // For fine-tuning the input volt master
+#define versionRegister_Offset 0x0b
+#define configRegister_Offset 0x0c
+#define identRegister_Offset 0x0d
 
 /*-------------------------Variables------------------------*/
-#define regMapSize  7
-uint8_t maxBytes = 2;
+#define regMapSize  14
+#define maxBytes 2
 #define longBytes 4
 byte regMap[regMapSize];
 byte regMapTemp[regMapSize];
@@ -26,10 +32,11 @@ byte longRcvd[longBytes];
 class pP_i2c {
   public:
     pP_i2c(uint8_t address=pP_i2c_address);
-
     void init();
     void i2cInput(int bytesReceived);
+  private:
+    char _i2cResponse;
 };
 
-#endif
-#endif
+#endif // I2C_INPUT
+#endif // _pP_i2c_h_