#include "modbus.h"

uint8_t state;
uint8_t packet[SIZE_MODBUS_PAKET];
uint8_t size_of_packet;
uint16_t crc_modbus;
uint16_t first_register_device;
uint16_t number_of_registers_required;
uint16_t crc_answ;
uint8_t modbus_answ[SIZE_MODBUS_PAKET];
uint16_t expected_crc;
unsigned long time;
uint8_t packet_accepted;
uint16_t last_possible_register;

uint16_t crc_chk(uint8_t *data, uint8_t length);
uint16_t bytes_unification(uint8_t high, uint8_t low);
uint8_t read_coils(uint8_t address, uint8_t value, uint8_t *modbus_answ);
uint8_t read_discrete_inputs(uint8_t address, uint8_t value, uint8_t *modbus_answ);
uint8_t read_holding_registers(uint8_t address, uint8_t value, uint8_t *modbus_answ);
uint8_t read_input_registers(uint8_t address, uint8_t value, uint8_t *modbus_answ);
uint8_t write_single_coil(uint8_t address, uint16_t coil_addr, int on, uint8_t *modbus_answ);
uint8_t write_single_register(uint8_t address, uint16_t register_addr, uint16_t new_value, uint8_t *modbus_answ);
uint8_t write_multiple_coils(uint8_t address, uint16_t first_coil_addr, uint16_t recorded_num, uint8_t *modbus_answ);
uint8_t write_multiple_registers(uint8_t address, uint16_t first_register_addr, uint16_t recorded_num, uint8_t *modbus_answ);
uint8_t modbus_err_answ(uint8_t address, uint8_t func_code, uint8_t err_code, uint8_t *modbus_answ);
void modbus_rtu_send(uint8_t answ_len);

void modbus_init(void)
{
    size_of_packet = 0;
    crc_modbus = 0xFFFF;
    state = 0;
}

void range_in_range(uint8_t a, uint8_t b, uint8_t x, uint8_t y)
{
	uint8_t c = a + b - 1;
	if (a >= x && a <= y) || (c >= x && c <= y)
	{
		return 0;
	}
	else
	{
		return 1;
	}
}

// Функция приема modbus-запроса RTU
void modbus_rtu(void)
{
    // состояние приема
    if (state == 0)
    {
        uint8_t data;
        // Размер пакета 0
        size_of_packet = 0;
        // Записываем время начала приема пакета
        time = get_millis();
        // Пакет не принят
        packet_accepted = 0;

        // Цикл приема символов
        while (!packet_accepted)
        {
            // Читаем символ из входящего кольцевого буфера
            // Кольцевой буфер не пуст
            if (count_elements(get_rb_receive()) > 0)
            {
                // Читаем символ из кольцевого буфера
                rb_get(get_rb_receive(), &data);
                if (size_of_packet == 0)
                {
                    // Пришел новый пакет
                    packet[0] = data;
                    size_of_packet = 1;
                    time = get_millis();
                }
                    
                else
                {
                    // Проверяем линейный буфер на переполнение
                    if (size_of_packet == SIZE_MODBUS_PAKET)
                    {
                        // Буфер переполнился
                        // Ждем новый пакет
                        size_of_packet = 0;
                        continue;
                    }
                    else
                    {
                        time = get_millis();
                        packet[size_of_packet] = data;
                        size_of_packet++;
                    }
                }
            }
            // Проверка временного интервала между символами
            else
            {
                if ((get_millis() - time) > MAX_PAUSE)
                {
                    // Превышен таймаут
                    // Пакет закончился
                    packet_accepted = 1;
                }
            }
        }
        // Проверяем размер пакета
        if (size_of_packet < 5)
        {
            size_of_packet = 0;
            crc_modbus = 0xFFFF;
            return;
        }

        // считаем контрольную сумму CRC и сравниваем с CRC в пакете
        expected_crc = bytes_unification(size_of_packet - 2, size_of_packet - 1);
        crc_modbus = crc_chk(packet, size_of_packet - 3);
        // проверка контрольной суммы CRC
        if (crc_modbus != expected_crc)
        {
            size_of_packet = 0;
            crc_modbus = 0xFFFF;
            // формируем ошибку произошла невосполнимая ошибка
            return;
        }
        // проверка адреса устройства
        else if (get_device_address() != packet[0])
        {
            size_of_packet = 0;
            crc_modbus = 0xFFFF;
            // формируем ошибку адреса
            return;
        }
        else
        {
            // Расчленяем пакет
            state = 1;
            return;
        }
    }
}

// Функция отправки modbus-запроса в кольцевой буфер
void modbus_rtu_send(uint8_t answ_len)
{
    size_of_packet = 0;
    // состояние отправки
    if (state == 1)
    {
        // цикл отправки символов
        while (state == 1)
        {
            // Отправка всех байт в кольцевой буфер
            rb_put(get_rb_transmit(), modbus_answ[size_of_packet]);

            // смещаем указатель
            size_of_packet++;
            if (size_of_packet == answ_len)
            {
                state = 0;
                size_of_packet = 0;
                crc_modbus = 0xFFFF;
                return;
            }
        }
    }
}

// Функция формирования modbus-ответа RTU
void modbus_answer()
{
    switch (packet[1])
    {
    case READ_COILS:
        first_register_device = bytes_unification(2, 3);
        number_of_registers_required = bytes_unification(4, 5);

        if (range_in_range(first_register_device, number_of_registers_required, COIL_1, COIL_4) && number_of_registers_required > 0)
		{
			modbus_rtu_send(modbus_err_answ(get_device_address(), packet[1], ILLEGAL_DATA_ADDRESS, &modbus_answ));
		}
        else
        {
            uint8_t value = 0;
            while (number_of_registers_required > 0)
            {
                switch (first_register_device)
                {
                case COIL_1:
                    value |= (gpio_read(PIN_COIL1));
                    break;
                case COIL_2:
                    value |= (gpio_read(PIN_COIL2));
                    break;
                case COIL_3:
                    value |= (gpio_read(PIN_COIL3));
                    break;
                case COIL_4:
                    value |= (gpio_read(PIN_COIL4));
                    break;
                }

                first_register_device++;
                number_of_registers_required--;
            }
            modbus_rtu_send(read_coils(get_device_address(), value, &modbus_answ));
        }

        break;
    case READ_DISCRETE_INPUTS:
        first_register_device = bytes_unification(2, 3);
        number_of_registers_required = bytes_unification(4, 5);

        if (range_in_range(first_register_device, number_of_registers_required, DISCRETE_INPUTS_1, DISCRETE_INPUTS_4) && number_of_registers_required > 0)
		{
			modbus_rtu_send(modbus_err_answ(get_device_address(), packet[1], ILLEGAL_DATA_ADDRESS, &modbus_answ));
		}
        else
        {
            uint8_t value = 0;
            while (number_of_registers_required > 0)
            {
                switch (first_register_device)
                {
                case DISCRETE_INPUTS_1:
                    value |= (gpio_read(PIN_DISCRETE_INPUT_1));
                    break;
                case DISCRETE_INPUTS_2:
                    value |= (gpio_read(PIN_DISCRETE_INPUT_2));
                    break;
                case DISCRETE_INPUTS_3:
                    value |= (gpio_read(PIN_DISCRETE_INPUT_3));
                    break;
                case DISCRETE_INPUTS_4:
                    value |= (gpio_read(PIN_DISCRETE_INPUT_4));
                    break;
                }

                first_register_device ++;
                number_of_registers_required--;
            }
            modbus_rtu_send(read_discrete_inputs(get_device_address(), value, &modbus_answ));
        }

        break;
    case READ_HOLDING_REGISTERS:
        first_register_device = bytes_unification(2, 3);
        number_of_registers_required = bytes_unification(4, 5);

        if (first_register_device != HOLDING_REGISTER_SLAVE_ADDRESS || number_of_registers_required != 1)
		{
			modbus_rtu_send(modbus_err_answ(get_device_address(), packet[1], ILLEGAL_DATA_ADDRESS, &modbus_answ));
		}
        else
        {
            modbus_rtu_send(read_holding_registers(get_device_address(), get_device_address(), &modbus_answ));
        }

        break;
    case READ_INPUT_REGISTERS:
        first_register_device = bytes_unification(2, 3);
        number_of_registers_required = bytes_unification(4, 5);

        if (first_register_device != INPUT_REGISTERS || number_of_registers_required != 1)
		{
			modbus_rtu_send(modbus_err_answ(get_device_address(), packet[1], ILLEGAL_DATA_ADDRESS, &modbus_answ));
		}
        else
        {
            modbus_rtu_send(read_input_registers(get_device_address(), get_adc_value(), &modbus_answ));
        }

        break;
    case WRITE_SINGLE_COIL:
        first_register_device = bytes_unification(2, 3);

        if (range_in_range(first_register_device, 1, COIL_1, COIL_4))
		{
			modbus_rtu_send(modbus_err_answ(get_device_address(), packet[1], ILLEGAL_DATA_ADDRESS, &modbus_answ));
		}
        else
        {
            uint8_t value = 0;
			switch (first_register_device)
			{
			case COIL_1:
				value = gpio_write(PIN_COIL1, packet[4]);
				break;
			case COIL_2:
				value = gpio_write(PIN_COIL2, packet[4]);
				break;
			case COIL_3:
				value = gpio_write(PIN_COIL3, packet[4]);
				break;
			case COIL_4:
				value = gpio_write(PIN_COIL4, packet[4]);
				break;
			}

            modbus_rtu_send(write_single_coil(get_device_address(), first_register_device, GPIORead(first_register_device), &modbus_answ));
        }

        break;
    case WRITE_SINGLE_REGISTER:
        first_register_device = bytes_unification(2, 3);

        if (first_register_device != HOLDING_REGISTER_SLAVE_ADDRESS)
		{
			modbus_rtu_send(modbus_err_answ(get_device_address(), packet[1], ILLEGAL_DATA_ADDRESS, &modbus_answ));
		}
        else
        {
            write_device_address(bytes_unification(4, 5));

            modbus_rtu_send(write_single_register(get_device_address(), first_register_device, get_device_address(), &modbus_answ));
        }

        break;
    default:
        modbus_rtu_send(modbus_err_answ(get_device_address(), packet[1], ILLEGAL_FUNCTION, &modbus_answ));
        break;
    }
}

// Функция вычисления контрольной суммы CRC
uint16_t crc_chk(uint8_t *data, uint8_t length)
{
    register int16_t j;
    register uint16_t reg_crc = 0xFFFF;
    while (length--)
    {
        reg_crc ^= *data++;
        for (j = 0; j < 8; j++)
        {
            if (reg_crc & 0x01)
            {
                reg_crc = (reg_crc >> 1) ^ 0xA001;
            }
            else
            {
                reg_crc = reg_crc >> 1;
            }
        }
    }
    return reg_crc;
}

// Функция объединения двух байт (старшего и младшего)
uint16_t bytes_unification(uint8_t high, uint8_t low)
{
    return (packet[high] << 8) | packet[low];
}

// Коды функций (для формирования ответа)

// Чтение данных с регистров флагов (функция с кодом 1)
uint8_t read_coils(uint8_t address, uint8_t value, uint8_t *modbus_answ)
{
    modbus_answ[0] = address;                // Адрес
    modbus_answ[1] = 0x01;                   // Код функции
    modbus_answ[2] = 0x01;                   // Кол-во передаваемых байт
    modbus_answ[3] = value;                  // Данные
    crc_answ = crc_chk(modbus_answ, 4);      // Подсчет контрольной суммы
    modbus_answ[4] = crc_answ & 0xFF;        // Первый байт контрольной суммы
    modbus_answ[5] = (crc_answ >> 8) & 0xFF; // Второй байт контрольной суммы
    return 6;
}

// Чтение данных с дискретных входов (функция с кодом 2)
uint8_t read_discrete_inputs(uint8_t address, uint8_t value, uint8_t *modbus_answ)
{
    modbus_answ[0] = address;                // Адрес
    modbus_answ[1] = 0x02;                   // Код функции
    modbus_answ[2] = 0x01;                   // Кол-во передаваемых байт
    modbus_answ[3] = value;                  // Данные
    crc_answ = crc_chk(modbus_answ, 4);      // Подсчет контрольной суммы
    modbus_answ[4] = crc_answ & 0xFF;        // Первый байт контрольной суммы
    modbus_answ[5] = (crc_answ >> 8) & 0xFF; // Второй байт контрольной суммы
    return 6;
}

// Чтение данных с регистров хранения (функция с кодом 3)
uint8_t read_holding_registers(uint8_t address, uint16_t value, uint8_t *modbus_answ)
{
    modbus_answ[0] = address;                // Адрес
    modbus_answ[1] = 0x03;                   // Код функции
    modbus_answ[2] = 0x01;                   // Кол-во передаваемых байт
    modbus_answ[3] = value;                  // Первый байт данных
    modbus_answ[4] = value >> 8;             // Второй байт данных
    crc_answ = crc_chk(modbus_answ, 5);      // Подсчет контрольной суммы
    modbus_answ[5] = crc_answ & 0xFF;        // Первый байт контрольной суммы
    modbus_answ[6] = (crc_answ >> 8) & 0xFF; // Второй байт контрольной суммы
    return 7;
}

// Чтение данных с регистров ввода (функция с кодом 4)
uint8_t read_input_registers(uint8_t address, uint16_t value, uint8_t *modbus_answ)
{
    modbus_answ[0] = address;                // Адрес
    modbus_answ[1] = 0x04;                   // Код функции
    modbus_answ[2] = 0x01;                   // Кол-во передаваемых байт
    modbus_answ[3] = value;                  // Первый байт данных
    modbus_answ[4] = value >> 8;             // Второй байт данных
    crc_answ = crc_chk(modbus_answ, 5);      // Подсчет контрольной суммы
    modbus_answ[5] = crc_answ & 0xFF;        // Первый байт контрольной суммы
    modbus_answ[6] = (crc_answ >> 8) & 0xFF; // Второй байт контрольной суммы
    return 7;
}

// Запись данных в регистр флагов (функция с кодом 5)
uint8_t write_single_coil(uint8_t address, uint16_t coil_addr, int on, uint8_t *modbus_answ)
{
    modbus_answ[0] = address;                 // Адрес
    modbus_answ[1] = 0x05;                    // Код функции
    modbus_answ[2] = (coil_addr >> 8) & 0xFF; // Старший байт адреса
    modbus_answ[3] = coil_addr & 0xFF;        // Младший байт адреса
    modbus_answ[4] = on ? 0xFF : 0x00;        // Значение, которое нужно записать
    modbus_answ[5] = 0x00;
    crc_answ = crc_chk(modbus_answ, 6);      // Подсчет контрольной суммы
    modbus_answ[6] = crc_answ & 0xFF;        // Первый байт контрольной суммы
    modbus_answ[7] = (crc_answ >> 8) & 0xFF; // Второй байт контрольной суммы
    return 8;
}

// Запись данных в регистр (функция с кодом 6)
uint8_t write_single_register(uint8_t address, uint16_t register_addr, uint16_t new_value, uint8_t *modbus_answ)
{
    modbus_answ[0] = address;                     // Адрес
    modbus_answ[1] = 0x06;                        // Код функции
    modbus_answ[2] = (register_addr >> 8) & 0xFF; // Старший байт адреса
    modbus_answ[3] = register_addr & 0xFF;        // Младший байт адреса
    modbus_answ[4] = (new_value >> 8) & 0xFF;     // Старший байт значения
    modbus_answ[5] = new_value & 0xFF;            // Младший байт значения
    crc_answ = crc_chk(modbus_answ, 6);           // Подсчет контрольной суммы
    modbus_answ[6] = crc_answ & 0xFF;             // Первый байт контрольной суммы
    modbus_answ[7] = (crc_answ >> 8) & 0xFF;      // Второй байт контрольной суммы
    return 8;
}

// Сообщение об ошибке
uint8_t modbus_err_answ(uint8_t address, uint8_t func_code, uint8_t err_code, uint8_t *modbus_answ)
{
    modbus_answ[0] = address;                // Адрес
    modbus_answ[1] = 0x80 + func_code;       // Код функции + ошибка
    modbus_answ[2] = err_code;               // Код ошибки
    crc_answ = crc_chk(modbus_answ, 3);      // Подсчет контрольной суммы
    modbus_answ[3] = crc_answ & 0xFF;        // Первый байт контрольной суммы
    modbus_answ[4] = (crc_answ >> 8) & 0xFF; // Второй байт контрольной суммы
    return 5;
}