diff --git a/modbus.c b/modbus.c
index a9ed127..d511ede 100644
--- a/modbus.c
+++ b/modbus.c
@@ -43,30 +43,44 @@ void modbus_timer(void)
 extern uint16_t measure_voltage();
 uint8_t modbus_analyse_and_answer(void) {
 	// TODO -> complete the modbus analyse and answer
-    uint8_t length = 0;
+    uint16_t length = 0;
     if(rx_buf[0] == modbusAddress){
         tx_buf[0] = rx_buf[0]; // Adress
         tx_buf[1] = rx_buf[1]; // Function
+        uint16_t adresseRegister = ((uint16_t)rx_buf[2] << 8) | rx_buf[3];
         
-        switch(rx_buf[1]){
+        switch(rx_buf[1]){  // Check the function from rx buffer
             case READ_INPUT_REGISTERS:
-                tx_buf[2] = 2; // Data length
-                tx_buf[4] = input_registers[0];     // LSB Data
-                tx_buf[3] = input_registers[0]>>8;  // MSB Data
-                length = 5;
-                // todo choose register
+                length = ((uint16_t)rx_buf[4] << 8) | rx_buf[5];
+                tx_buf[2] = (uint8_t)(length*2);        // Data length
+                for(uint16_t i = 0; i < length; i++){   // Data
+                    tx_buf[i*2+4] = input_registers[adresseRegister+i];
+                    tx_buf[i*2+3] = (input_registers[adresseRegister+i] >> 8);
+                }
+                length*=2;
+                length+=3;
                 break;
             case READ_HOLDING_REGISTERS:
-                //todo
+                length = ((uint16_t)rx_buf[4] << 8) | rx_buf[5];
+                tx_buf[2] = (uint8_t)(length*2);        // Data length
+                for(uint16_t i = 0; i < length; i++){   // Data
+                    tx_buf[i*2+4] = holding_registers[adresseRegister+i];
+                    tx_buf[i*2+3] = (holding_registers[adresseRegister+i] >> 8);
+                }
+                length*=2;
+                length+=3;
                 break;
             case WRITE_SINGLE_REGISTER:
-                //todo
+                holding_registers[adresseRegister] = ((uint16_t)rx_buf[4] << 8) | rx_buf[5];
+                for (int i = 2; i <= 5; i++) {
+                    tx_buf[i] = rx_buf[i];
+                    length = i+1;
+                }
                 break;
-                // todo CRC
         }
         
-        
     }
+       
     rx_buf[0] = 0;
     modbus_send(length);
   
@@ -82,19 +96,23 @@ void modbus_char_recvd(void)
 void modbus_send(uint8_t length)
 {
     
-	uint16_t temp16; 
-	uint8_t i;
+	uint16_t crc = CRC16(tx_buf, length);
+	
+    tx_buf[length] = crc;
+    tx_buf[length+1] = crc >> 8;
 
-	// TODO -> complete modbus crc calculation
 	length += 2; // add 2 CRC bytes for total size
 
 	// For all the bytes to be transmitted
-  uart_send(tx_buf,length);
+    for (uint8_t i = 0; i < length; i++){
+          EUSART1_Write(tx_buf[i]);
+      }
 }
 
 void modbus_init(uint8_t address)
 {
 	modbusAddress = address;
+    holding_registers[1] = address;
     EUSART1_SetRxInterruptHandler(modbus_char_recvd);
     TMR0_SetInterruptHandler(modbus_timer);
 }