/*
 * modbus_logic.c
 *
 *  Created on: Jul 25, 2024
 *      Author: User
 */


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

extern UART_HandleTypeDef huart1;
extern DMA_HandleTypeDef hdma_usart1_rx;

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

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;


void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{
  if (huart->Instance == USART1)
  {
      if (recv_buffer[5] != BOARD_ADRESS)
      {
        cmd_receiving = RECV_ERROR;
        return;
      }
      // Check function code
      if (recv_buffer[6] == 3 || recv_buffer[6] == 4 || recv_buffer[6] == 6)
      {
        if(Size==MODBUS_HEADER_SIZE)cmd_receiving = RECV_READ_COMPLETE;
        return;
      }
      else if (recv_buffer[6] == 0x10)
      {
        uint16_t pdu_size = *((uint16_t*)(recv_buffer + 2));
        uint16_t remaining_size = pdu_size - (MODBUS_HEADER_SIZE - 4) + 2;
        if (remaining_size + MODBUS_HEADER_SIZE == Size)
          cmd_receiving = RECV_READ_COMPLETE;
        else
        {
          cmd_receiving = RECV_ERROR;
          tranfer_errors_count++;
        }
        return;
      }
      else
      {
        tranfer_errors_count++;
        cmd_receiving = RECV_ERROR;
        return;
      }
  }
}

void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
  if (huart->Instance == USART1)
  {
    cmd_sending = SEND_IDLE;
    TXEN_OFF;
  }

}

void modbus_loop_iterate()
{
  if (cmd_sending == SEND_IDLE)
  {
    if (cmd_receiving == RECV_ERROR)
    {
      cmd_receiving = RECV_IDLE;
    }
    if (cmd_receiving == RECV_IDLE)
    {
      if(HAL_UARTEx_ReceiveToIdle_DMA(&huart1, recv_buffer, BUFFERS_SIZE)==HAL_OK)
      {
        cmd_receiving = RECV_READ_HEADER;
        __HAL_DMA_DISABLE_IT(&hdma_usart1_rx, DMA_IT_HT);
      }
    }
    else if (cmd_receiving == RECV_READ_COMPLETE)
    {
      process_incoming_packet();
      cmd_receiving = RECV_IDLE;
    }
  }
  else if (cmd_sending == SEND_BUSY && bytes_to_send != 0)
  {
    TXEN_ON;
    HAL_UART_Transmit_DMA(&huart1, send_buffer, bytes_to_send);
    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++;
  }
}