#include <iostream>
#include <thread>
#include <chrono>
#include <string>
#include <vector>
#include <stdexcept>
#include <iomanip> // For std::setw, std::setfill, std::hex, std::dec

// Include the new header files
#include "modbus/modbus_rtu_client.h"
#include "modbus/modbus_temperature_humidity.h"

int main(int argc, char* argv[]) {
    std::cout << "--- Modbus RTU Sensor Client ---" << std::endl;

    std::string port_path = "/dev/ttyS7";
    unsigned int baud_rate = 9600;
    uint8_t slave_id = 0x01; 

    if (argc > 1) {
        port_path = argv[1];
    }
    if (argc > 2) {
        baud_rate = static_cast<unsigned int>(std::stoul(argv[2]));
    }
    if (argc > 3) {
        slave_id = static_cast<uint8_t>(std::stoul(argv[3]));
    }

    ModbusRTUClient client;

    // Set and open serial port settings
    if (!client.setPortSettings(port_path, baud_rate, 8, 1, 'N')) {
        std::cerr << "Failed to set up serial port. Exiting." << std::endl;
        return 1;
    }

    std::cout << "\nStarting continuous Modbus polling. Press Ctrl+C to exit." << std::endl;

    while (true) {
        try {
            std::vector<uint16_t> registers;
            
            // Read Temperature (FC 0x03, address 0x0000, 1 register)
            registers = client.readHoldingRegisters(slave_id, 0x0000, 1);
            printRegisters("Temperature", registers);

            // Read Humidity (FC 0x03, address 0x0001, 1 register)
            registers = client.readHoldingRegisters(slave_id, 0x0001, 1);
            printRegisters("Humidity", registers);

            // Read Temperature and Humidity (FC 0x03, address 0x0000, 2 registers)
            registers = client.readHoldingRegisters(slave_id, 0x0000, 2);
            printRegisters("Temperature, Humidity (Combined)", registers);

            // Read Temperature Alarm Status (FC 0x04, address 0x0030, 1 register)
            registers = client.readInputRegisters(slave_id, 0x0030, 1);
            printRegisters("Temperature Alarm Status", registers);

            // Read Humidity Alarm Status (FC 0x04, address 0x0031, 1 register)
            registers = client.readInputRegisters(slave_id, 0x0031, 1);
            printRegisters("Humidity Alarm Status", registers);
            
            // Read Device Info (using a dummy address for example, adjust as per device docs)
            // Assuming 0x0031 is some device common info, not strictly alarm
            registers = client.readHoldingRegisters(slave_id, 0x0031, 1);
            printRegisters("Device Info", registers); 

            // Example: Read Modbus Address (FC 0x03, address 0x1000, 1 register)
            // Note: This uses the low-level ModbusRequest/ModbusResponse directly
            // to show how it can be used if the high-level API is not sufficient or for specific needs.
            ModbusRequest req_addr = ModbusRequest::createReadRegistersRequest(slave_id, 0x03, 0x1000, 1);
            ModbusResponse res_addr = client.sendAndReceive(req_addr);
            // Check if response is valid before parsing
            if (!res_addr.is_exception) {
                printRegisters("Modbus Address", res_addr.getRegisters());
            } else {
                std::cerr << "Failed to read Modbus Address: ";
                res_addr.print(); // Print detailed exception
            }

            // Example: Write a single register (FC 0x06, address 0x0002, value 123)
            // For demonstration, let's assume register 0x0002 can be written.
            // Be careful when writing to actual hardware!
            // std::cout << "Attempting to write 123 to register 0x0002..." << std::endl;
            // client.writeSingleRegister(slave_id, 0x0002, 123);
            // std::cout << "Write single register complete (or exception thrown if failed)." << std::endl;


        } catch (const std::exception& e) {
            std::cerr << "--- Error during Modbus communication: " << e.what() << " ---" << std::endl;
        }

        std::this_thread::sleep_for(std::chrono::seconds(2)); // Poll every 2 seconds
    }

    client.closePort();
    std::cout << "\nProgram terminated." << std::endl;
    return 0;
}