#include "modbus.h"
#include "crc.h"
#include "uart.h"
#include <xc.h>
#include <stdint.h>
#include <stdio.h>

// Modbus functions
#define READ_INPUT_REGISTERS    0x04
#define READ_HOLDING_REGISTERS  0x03
#define WRITE_SINGLE_REGISTER   0x06

// Modbus data model
uint8_t modbusAddress;
uint16_t input_registers[2];
uint16_t holding_registers[2];


// Modbus error codes
#define ILLEGAL_FUNCTION		1
#define ILLEGAL_DATA_ADDRESS	2
#define ILLEGAL_DATA_VALUE		3
#define SLAVE_DEVICE_FAILURE	4

/**
 * Buffers for serial receive and send operations 
 * which are more than one byte long
 **/
uint8_t rx_buf[256];
uint8_t tx_buf[256];

// Current position pointer for storing receive position
uint8_t recPtr = 0;

void modbus_timer(void)
{
	INTCONbits.TMR0IF = 0;
    recPtr = 0;   
    TMR0_StopTimer();
    modbus_analyse_and_answer();
   
}
extern uint16_t measure_voltage();
uint8_t modbus_analyse_and_answer(void) {
	// TODO -> complete the modbus analyse and answer
    uint8_t length = 0;
    if(rx_buf[0] == modbusAddress){
        tx_buf[0] = rx_buf[0]; // Adress
        tx_buf[1] = rx_buf[1]; // Function
        
        switch(rx_buf[1]){
            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
                break;
            case READ_HOLDING_REGISTERS:
                //todo
                break;
            case WRITE_SINGLE_REGISTER:
                //todo
                break;
                // todo CRC
        }
        
        
    }
    rx_buf[0] = 0;
    modbus_send(length);
  
}

void modbus_char_recvd(void)
{
	rx_buf[recPtr++] = RCREG1;
    TMR0_Reload();
    TMR0_StartTimer();
}

void modbus_send(uint8_t length)
{
    
	uint16_t temp16; 
	uint8_t i;

	// 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);
}

void modbus_init(uint8_t address)
{
	modbusAddress = address;
    EUSART1_SetRxInterruptHandler(modbus_char_recvd);
    TMR0_SetInterruptHandler(modbus_timer);
}