/*
 * modbus_logic.c
 *
 *  Created on: 11 ���. 2019 �.
 */

#include <stdint.h>
#include "main.h"
#include "uart.h"
#include "modbus_logic.h"

volatile uint8_t recv_buffer[BUFFERS_SIZE];
volatile uint8_t send_buffer[BUFFERS_SIZE];

extern uint16_t uart_rx_counter;
extern uint16_t uart_rx_len;

uint16_t bytes_to_send = 0;
uint16_t last_rx_time = 0xFFFF;
enum recv_state cmd_receiving = RECV_IDLE;
enum send_state cmd_sending = SEND_IDLE;
uint16_t tranfer_errors_count;
extern uint16_t led_time_act;

static void process_incoming_packet();

void UART_RxCpltCallback(void)
{
  uint16_t pdu_size;
  uint16_t remaining_size;
  if (cmd_receiving == RECV_READ_HEADER)
  {
      // Check destination address
	  if (recv_buffer[5] != BOARD_ADRESS)
	  {

//		  if(recv_buffer[5]==BOARD_ADRESS_Panel || recv_buffer[5]==BOARD_ADRESS_DAC )
//		  {

	//		  memset(&recv_buffer[0],0,BUFFERS_SIZE);

			  last_rx_time =0;

//		  }
		  cmd_receiving = RECV_ERROR;
		  return;
	  } 
      
	  // Check function code
	  if (recv_buffer[6] == 3 || recv_buffer[6] == 4 || recv_buffer[6] == 6)
	  {
		  cmd_receiving = RECV_READ_COMPLETE;
		  return;
	  }
	  else if (recv_buffer[6] == 0x10)
	  {
		  cmd_receiving = RECV_READ_DATA;
		  pdu_size = *((uint16_t*)(recv_buffer + 2));
		  // Receive remaining PDU and checksum
		  remaining_size = pdu_size - (MODBUS_HEADER_SIZE - 4) + 2;
		  if (remaining_size + MODBUS_HEADER_SIZE <= BUFFERS_SIZE)
			  UART_Receive_IT(recv_buffer + MODBUS_HEADER_SIZE, remaining_size, 5000);
		  else
			  tranfer_errors_count++;
		  return;
	  }
	  else
	  {
		  tranfer_errors_count++;
		  cmd_receiving = RECV_ERROR;
		  return;
	  }
  }
  else if (cmd_receiving == RECV_READ_DATA)
  {
	  cmd_receiving = RECV_READ_COMPLETE;
  }
}

void UART_TxCpltCallback()
{
	cmd_sending = SEND_IDLE;
}

void modbus_loop_iterate()
{
	if (cmd_sending == SEND_IDLE)
	{
		if (cmd_receiving == RECV_ERROR)
		{
//			HAL_Delay(10); // poor man's way to synchronize packets
            if(last_rx_time>=BUS_IDLE_TIME)
           {
                cmd_receiving = RECV_IDLE;
            //    memset(&recv_buffer[0],0,BUFFERS_SIZE);
                //TXEN_485 = 1;
                //delay_us(10);
                //TXEN_485 = 0;
           }
		}
		if (cmd_receiving == RECV_IDLE)
		{
			cmd_receiving = RECV_READ_HEADER;
			UART_Receive_IT(recv_buffer, MODBUS_HEADER_SIZE, 5000);
		}
		else if (cmd_receiving == RECV_READ_COMPLETE)
		{
			process_incoming_packet();
			cmd_receiving = RECV_IDLE;
		}
	}
	else if (cmd_sending == SEND_BUSY && bytes_to_send != 0)
	{
		UART_Transmit_IT(send_buffer, bytes_to_send, 1000);
		bytes_to_send = 0;
	}
}

static uint16_t checksum(int size)
{
	uint16_t index;
    uint16_t checksum = 0;
	for (index = 0; index < size; index++)
		checksum += send_buffer[index];
	return checksum;
}

void process_incoming_packet()
{
    uint8_t source;
    uint8_t dest;
	uint8_t func_code;
	uint16_t checksum_recv;
    uint16_t transaction_id;
    uint16_t pdu_size;
    uint16_t checksum_real;
    uint16_t response_pdu_length;   
    uint16_t address;     
    uint8_t write_result;  
    uint16_t size;
    uint16_t i;

    led_time_act=100;


	dest = recv_buffer[5];
    if(dest!=BOARD_ADRESS)return;
    
	transaction_id = *((uint16_t*)(recv_buffer));
	pdu_size = *((uint16_t*)(recv_buffer + 2));
	if (pdu_size + 6 > BUFFERS_SIZE)
	{
		tranfer_errors_count++;
		return;
	}
	source = recv_buffer[4];
	func_code = recv_buffer[6];
	//checksum_recv = *((uint16_t*)(recv_buffer + 4 + pdu_size));
	checksum_recv = recv_buffer[4 + pdu_size];
	checksum_recv += ((uint16_t)recv_buffer[5 + pdu_size])<<8;

	checksum_real = 0;
	for (i = 0; i < 4 + pdu_size; i++)
		checksum_real += recv_buffer[i];
	if (checksum_recv != checksum_real)
	{
		tranfer_errors_count++;
		return;
	}

	// Prepare response header
	*(uint16_t*)(send_buffer) = transaction_id;
	send_buffer[4] = BOARD_ADRESS;
	send_buffer[5] = source;

	response_pdu_length = 0;
	if ((func_code == 3 || func_code == 4) && pdu_size == 7) // Read holding/input registers
	{
		uint16_t start_address = *((uint16_t*)(recv_buffer + 7));
		uint16_t quantity = *((uint16_t*)(recv_buffer + 9));
		if (quantity > MAX_REGS_PER_QUERY)
		{
			tranfer_errors_count++;
			return;
		}
		response_pdu_length = 2 * quantity + 1 + 3;
		send_buffer[6] = func_code;
		send_buffer[7] = 0; // bytes counter
		for (address = start_address; address < start_address + quantity; address++)
		{
			uint16_t value;
			uint8_t read_result = read_register(address, &value);
			if (read_result != 0)
			{
				send_buffer[6] = func_code + 0x80;
				send_buffer[7] = read_result;
				response_pdu_length = 4;
				break;
			}
			send_buffer[7] += 2;
			*((uint16_t*)(&(send_buffer[8 + 2 * (address - start_address)]))) = value;
		}
	}
	else if (func_code == 6 && pdu_size == 7) // Write holding register
	{
		uint16_t address = *((uint16_t*)(recv_buffer + 7));
		uint16_t value = *((uint16_t*)(recv_buffer + 9));
		response_pdu_length = 7;
		send_buffer[6] = func_code;
		write_result = write_register(address, value);
		if (write_result == 0)
		{
			//*((uint16_t*)(send_buffer + 7)) = address;
			send_buffer[7] = (uint8_t)(address&0x00FF);
			send_buffer[8] = (uint8_t)((address&0xFF00)>>8);
			//*((uint16_t*)(send_buffer + 9)) = value;
			send_buffer[9] = (uint8_t)(value&0x00FF);
			send_buffer[10] = (uint8_t)((value&0xFF00)>>8);
		}
		else
		{
			response_pdu_length = 4;
			send_buffer[6] = 0x86;
			send_buffer[7] = write_result;
		}
	}
	else if (func_code == 0x10) // Write multiple registers
	{
		uint16_t start_address = *((uint16_t*)(recv_buffer + 7));
		uint16_t quantity = *((uint16_t*)(recv_buffer + 9));
		uint8_t byte_count = recv_buffer[11];
		if (pdu_size != 3 + 5 + quantity * 2 || quantity * 2 != byte_count)
		{
			tranfer_errors_count++;
			return;
		}
		response_pdu_length = 7;
		send_buffer[6] = func_code;
		//*((uint16_t*)(send_buffer + 7)) = start_address;
		send_buffer[7] = (uint8_t)(start_address&0x00FF);
		send_buffer[8] = (uint8_t)((start_address&0xFF00)>>8);
		//*((uint16_t*)(send_buffer + 9)) = quantity;
		send_buffer[9] = (uint8_t)(quantity&0x00FF);
		send_buffer[10] = (uint8_t)((quantity&0xFF00)>>8);
		
		for (address = start_address; address < start_address + quantity; address++)
		{
			uint16_t value = *((uint16_t*)(recv_buffer + 12u + 2u * (address - start_address)));
			write_result = write_register(address, value);
			if (write_result != 0)
			{
				response_pdu_length = 4;
				send_buffer[6] = 0x90;
				send_buffer[7] = write_result;
				break;
			}
		}
	}
	else
	{
		tranfer_errors_count++;
		return;
	}

	//*((uint16_t*)(send_buffer + 2)) = response_pdu_length;
	send_buffer[2] = (uint8_t)(response_pdu_length&0x00FF);
	send_buffer[3] = (uint8_t)((response_pdu_length&0xFF00)>>8);

	size = 4 + response_pdu_length;
	//*((uint16_t*)(send_buffer + size)) = checksum(size);
	uint16_t s = checksum(size);
	send_buffer[size] = (uint8_t)(s&0x00FF);
	send_buffer[size+1] = (uint8_t)((s&0xFF00)>>8);

	if (cmd_sending == SEND_IDLE)
	{
		bytes_to_send = size + 2;
		cmd_sending = SEND_BUSY;
	}
	else
	{
		tranfer_errors_count++;
	}
}