/* USER CODE BEGIN Header */
/**
 * Begin Comments
 * @brief Lightweight Modbus RTU slave driver for STM32 + FreeRTOS
 *
 * This module implements a simple, real-time friendly Modbus RTU slave
 * using UART idle-line detection and RS485 GPIO control. It supports
 * function codes 0x03 and 0x10 and is designed to run as a cooperative
 * background task in embedded systems using STM32 HAL and FreeRTOS.
 *
 * Author: Scott Leonard
 * License: Unlicense
 */

/* USER CODE END Header */

#include "main.h"
#include "app_tasks.h"

#include <string.h>
#include <stdbool.h>

#include "FreeRTOS.h"
#include "task.h"
#include "cmsis_os.h"


#define RX_FRAME_LENGTH   256
#define TX_FRAME_LENGTH   256
#define TABLE_SIZE        128
#define SLAVE_ID          0x01

enum {
    READ_HOLDING_REGISTERS = 0x03,
    WRITE_HOLDING_REGISTERS = 0x10
};

#define ILLEGAL_FUNCTION      0x01
#define ILLEGAL_DATA_ADDRESS  0x02
#define ILLEGAL_DATA_VALUE    0x03


extern UART_HandleTypeDef huart2;

uint8_t  request_frame[RX_FRAME_LENGTH];
uint8_t  response_frame[TX_FRAME_LENGTH];
uint16_t modbus_table[TABLE_SIZE] = {0};

void rs485_dir_tx(void) {
    HAL_GPIO_WritePin(GPIOF, PF0_DE_NRE_Pin, GPIO_PIN_SET);   // HIGH = TX
    __NOP(); __NOP(); __NOP(); __NOP();                       // Delay for direction switch
}

void rs485_dir_rx(void) {
    HAL_GPIO_WritePin(GPIOF, PF0_DE_NRE_Pin, GPIO_PIN_RESET); // LOW = RX
}

void app_task_Modbus(void)
{
    static uint8_t modbus_initialized = 0;
    if (!modbus_initialized) {
        init_modbus_system();
        HAL_UARTEx_ReceiveToIdle_IT(&huart2, request_frame, RX_FRAME_LENGTH);
        modbus_initialized = 1;
    }
    osDelay(10);
}

uint16_t calculate_modbus_crc(uint8_t *data, uint16_t length) {
    uint16_t crc = 0xFFFF;
    for (uint16_t pos = 0; pos < length; pos++) {
        crc ^= (uint16_t)data[pos];
        for (uint8_t i = 8; i != 0; i--) {
            if ((crc & 0x0001) != 0) {
                crc >>= 1;
                crc ^= 0xA001;
            } else {
                crc >>= 1;
            }
        }
    }
    return crc;
}

void send_modbus_data(uint8_t *data, int size) {
    uint16_t crc = calculate_modbus_crc(data, size);
    data[size] = crc & 0xFF;
    data[size+1] = (crc>>8) & 0xFF;
    rs485_dir_tx();
    HAL_UART_Transmit(&huart2, data, size+2, 100);
    rs485_dir_rx();
}

void modbus_exception(uint8_t exceptionCode) {
    response_frame[0] = request_frame[0];
    response_frame[1] = request_frame[1] | 0x80;
    response_frame[2] = exceptionCode;
    send_modbus_data(response_frame, 3);
}

uint8_t table_read(void) {
    uint16_t startAddr = ((request_frame[2]<<8) | request_frame[3]);
    uint16_t numRegs = ((request_frame[4]<<8) | request_frame[5]);
    
    if ((numRegs < 1) || (numRegs > 125)) {
        modbus_exception(ILLEGAL_DATA_VALUE);
        return 0;
    }
    
    uint16_t endAddr = startAddr + numRegs - 1;
    if (endAddr > TABLE_SIZE - 1) {
        modbus_exception(ILLEGAL_DATA_ADDRESS);
        return 0;
    }
    
    response_frame[0] = request_frame[0];
    response_frame[1] = request_frame[1];
    response_frame[2] = numRegs * 2;
    int indx = 3;
    
    for (int i = 0; i < numRegs; i++) {
        response_frame[indx++] = (modbus_table[startAddr] >> 8) & 0xFF;
        response_frame[indx++] = modbus_table[startAddr] & 0xFF;
        startAddr++;
    }
    
    send_modbus_data(response_frame, indx);
    return 1;
}

uint8_t table_write(void) {
    uint16_t startAddr = ((request_frame[2]<<8) | request_frame[3]);
    uint16_t numRegs = ((request_frame[4]<<8) | request_frame[5]);
    uint8_t byteCount = request_frame[6];
    
    if ((numRegs < 1) || (numRegs > 123)) {
        modbus_exception(ILLEGAL_DATA_VALUE);
        return 0;
    }
    
    if (byteCount != (numRegs * 2)) {
        modbus_exception(ILLEGAL_DATA_VALUE);
        return 0;
    }

    uint16_t endAddr = startAddr + numRegs - 1;
    if (endAddr > TABLE_SIZE - 1) {
        modbus_exception(ILLEGAL_DATA_ADDRESS);
        return 0;
    }

    int indx = 7;
    for (int i = 0; i < numRegs; i++) {
        uint8_t high_byte = request_frame[indx++];
        uint8_t low_byte = request_frame[indx++];
        modbus_table[startAddr++] = (high_byte << 8) | low_byte;
    }
    
    response_frame[0] = request_frame[0];
    memcpy(&response_frame[1], &request_frame[1], 5);

    
    send_modbus_data(response_frame, 6);
    return 1;
}

void process_modbus_message(uint16_t Size) {
    if (Size < 8) return;
    if (request_frame[0] != SLAVE_ID) return;
    
    uint16_t receivedCRC = request_frame[Size-2] | (request_frame[Size-1] << 8);
    uint16_t calculatedCRC = calculate_modbus_crc(request_frame, Size-2);
    if (receivedCRC != calculatedCRC) return;
    
    switch (request_frame[1]) {
        case READ_HOLDING_REGISTERS:
            table_read();
            break;
        case WRITE_HOLDING_REGISTERS:
            table_write();
            break;
        default:
            modbus_exception(ILLEGAL_FUNCTION);
            break;
    }
}

void init_modbus_system(void) {

    memset(request_frame,  0, sizeof(request_frame));
    memset(response_frame, 0, sizeof(response_frame));
    rs485_dir_rx();  // Start in RX mode
}

void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart)
{
    if (huart->Instance == USART2) {
        __HAL_UART_CLEAR_FLAG(huart, UART_CLEAR_PEF | UART_CLEAR_FEF | UART_CLEAR_NEF | UART_CLEAR_OREF);
        HAL_UARTEx_ReceiveToIdle_IT(&huart2, request_frame, RX_FRAME_LENGTH);
    }
}