#include <Wire.h>
#include <LiquidCrystal_I2C.h>

#define MASTER_TX 17
#define MASTER_RX 18

#define SLAVE_RX 16
#define SLAVE_TX 19

#define RS485_DIR_PIN 5
#define SENSOR_PIN 34

// --- КАРТА РЕГІСТРІВ SLAVE ---
// 10 регістрів (адреси від 0 до 9)
uint16_t holdingRegisters[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

int packetCounter = 1;
LiquidCrystal_I2C lcd(0x27, 16, 2);

// ==========================================
// ФУНКЦІЯ РОЗРАХУНКУ CRC-16 (Modbus RTU)
// ==========================================
uint16_t calculateCRC(byte *data, int len) {
  uint16_t crc = 0xFFFF;
  for (int pos = 0; pos < len; pos++) {
    crc ^= (uint16_t)data[pos];    
    for (int i = 8; i != 0; i--) { 
      if ((crc & 0x0001) != 0) {   
        crc >>= 1;                 
        crc ^= 0xA001;             
      } else {
        crc >>= 1;                 
      }
    }
  }
  return crc;
}

void setup() {
  Serial.begin(115200);
  delay(1000);
  
  // Налаштування LCD
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Reg[1] Value:");

  pinMode(RS485_DIR_PIN, OUTPUT);
  digitalWrite(RS485_DIR_PIN, LOW);

  // Налаштовуємо серійні порти (Таймаут 50мс)
  Serial1.begin(115200, SERIAL_8N1, MASTER_RX, MASTER_TX);
  Serial2.begin(115200, SERIAL_8N1, SLAVE_RX, SLAVE_TX);
  Serial2.setTimeout(50); 
  
  Serial.println("\n--- Запуск: Modbus RTU (Hardcore Mode 90+) ---");
}

void loop() {
  // ==========================================
  // 1. MASTER: ФОРМУВАННЯ ТА ВІДПРАВКА
  // ==========================================
  int sensorValue = analogRead(SENSOR_PIN);
  uint16_t targetReg = 1; // Основний робочий регістр
  
  byte request[8];
  int reqLen = 8;

  // Чергування логіки Master для демонстрації всіх функцій
  if (packetCounter % 5 == 0) {
    // --- ДЕМОНСТРАЦІЯ ПОМИЛКИ (Exception 0x02) ---
    targetReg = 15; // Неіснуючий регістр!
    Serial.println("\n[Master] !!! Навмисна помилка: запис у регістр 15 !!!");
    request[0] = 0x01; request[1] = 0x06;
    request[2] = (targetReg >> 8) & 0xFF; request[3] = targetReg & 0xFF;
    request[4] = (sensorValue >> 8) & 0xFF; request[5] = sensorValue & 0xFF;
  } 
  else if (packetCounter % 2 == 0) {
    // --- ДЕМОНСТРАЦІЯ ЧИТАННЯ (0x03) ---
    Serial.println("\n--------------------------------------------------");
    Serial.printf("[Master] Запит на ЧИТАННЯ (0x03) з регістра %d\n", targetReg);
    request[0] = 0x01; request[1] = 0x03;
    request[2] = (targetReg >> 8) & 0xFF; request[3] = targetReg & 0xFF;
    request[4] = 0x00; request[5] = 0x01; // Читаємо 1 регістр
  } 
  else {
    // --- ДЕМОНСТРАЦІЯ ЗАПИСУ (0x06) ---
    Serial.println("\n--------------------------------------------------");
    Serial.printf("[Master] Відправив значення датчика: %d у регістр: %d\n", sensorValue, targetReg);
    request[0] = 0x01; request[1] = 0x06;
    request[2] = (targetReg >> 8) & 0xFF; request[3] = targetReg & 0xFF;
    request[4] = (sensorValue >> 8) & 0xFF; request[5] = sensorValue & 0xFF;
  }

  // Рахуємо динамічний CRC-16
  uint16_t crc = calculateCRC(request, 6);
  request[6] = crc & 0xFF;                 // CRC (Low)
  request[7] = (crc >> 8) & 0xFF;          // CRC (High)

  // Відправка
  digitalWrite(RS485_DIR_PIN, HIGH);
  Serial1.write(request, reqLen);
  Serial1.flush();
  digitalWrite(RS485_DIR_PIN, LOW);
  delay(50); 

  // ==========================================
  // 2. SLAVE: ПРИЙОМ ТА ПЕРЕВІРКА
  // ==========================================
  if (Serial2.available() >= 8) {
    byte buffer[32];
    int len = Serial2.readBytes(buffer, 32);

    // Перевірка CRC
    uint16_t receivedCRC = buffer[len-2] | (buffer[len-1] << 8);
    uint16_t calculatedCRC = calculateCRC(buffer, len - 2);
    
    if (receivedCRC != calculatedCRC) {
      Serial.println("[Slave] ВІДХИЛЕНО! Помилка CRC-16.");
      return; 
    }

    if (buffer[0] != 0x01) return; // Перевірка ID (0x01)

    int funcCode = buffer[1];
    int regAddr = (buffer[2] << 8) | buffer[3];

    // --- ОБРОБКА ФУНКЦІЇ 0x06 (ЗАПИС) ---
    if (funcCode == 0x06) {
      int regVal = (buffer[4] << 8) | buffer[5];

      if (regAddr >= 10) {
        // Exception 0x02
        Serial.printf("[Slave] Exception 0x02: Неіснуюча адреса %d!\n", regAddr);
        byte exception[5] = {0x01, 0x86, 0x02, 0, 0}; // 0x86 = 0x06 + 0x80
        uint16_t excCRC = calculateCRC(exception, 3);
        exception[3] = excCRC & 0xFF; exception[4] = (excCRC >> 8) & 0xFF;
        Serial2.write(exception, 5); 
      } else {
        // Успішний запис
        holdingRegisters[regAddr] = regVal;
        Serial.printf("[Slave] Успіх! Записано значення %d у регістр %d.\n", regVal, regAddr);
        
        Serial2.write(buffer, len); // Echo відповідь

        // Вивід на LCD
        if (regAddr == 1) {
          lcd.setCursor(0, 1);
          lcd.print(holdingRegisters[1]);
          lcd.print("    "); 
        }
      }
    }
    // --- ОБРОБКА ФУНКЦІЇ 0x03 (ЧИТАННЯ) ---
    else if (funcCode == 0x03) {
      int regCount = (buffer[4] << 8) | buffer[5];
      
      if (regAddr + regCount > 10) {
        // Exception 0x02
        Serial.printf("[Slave] Exception 0x02 (Читання): Вихід за межі пам'яті!\n");
        byte exception[5] = {0x01, 0x83, 0x02, 0, 0}; // 0x83 = 0x03 + 0x80
        uint16_t excCRC = calculateCRC(exception, 3);
        exception[3] = excCRC & 0xFF; exception[4] = (excCRC >> 8) & 0xFF;
        Serial2.write(exception, 5);
      } else {
        // Формування відповіді з даними
        byte response[32];
        response[0] = 0x01;
        response[1] = 0x03;
        response[2] = regCount * 2; // Кількість байтів даних
        
        int idx = 3;
        for (int i = 0; i < regCount; i++) {
          response[idx++] = (holdingRegisters[regAddr + i] >> 8) & 0xFF; // High byte
          response[idx++] = holdingRegisters[regAddr + i] & 0xFF;        // Low byte
        }
        
        uint16_t respCRC = calculateCRC(response, idx);
        response[idx++] = respCRC & 0xFF;
        response[idx++] = (respCRC >> 8) & 0xFF;
        Serial2.write(response, idx);
        Serial.println("[Slave] Успіх! Відправлені дані на запит читання (0x03).");
      }
    }
  }

  packetCounter++;
  delay(1500);
}