#include <iostream>
#include <vector>
#include <iomanip> // For std::hex, std::dec, std::fixed, std::setprecision
#include <map>     // For baudrate mapping
#include <string>  // For string manipulation
#include <unistd.h> // For read, write, close functions
#include <fcntl.h>  // File control definitions
#include <termios.h> // POSIX terminal control definitions
#include <cstring>  // For strerror

// --------------------------- Modbus Utility Functions ---------------------------

// Function to calculate Modbus RTU CRC16
uint16_t calculate_modbus_crc(const std::vector<uint8_t>& data) {
    uint16_t crc = 0xFFFF;
    for (uint8_t byte : data) {
        crc ^= byte;
        for (int i = 0; i < 8; ++i) {
            if (crc & 0x0001) {
                crc >>= 1;
                crc ^= 0xA001;
            } else {
                crc >>= 1;
            }
        }
    }
    return crc;
}

// Function to convert MSB, LSB to a signed 16-bit integer
int16_t bytesToSignedInt16(uint8_t msb, uint8_t lsb) {
    uint16_t val = (static_cast<uint16_t>(msb) << 8) | lsb;
    return static_cast<int16_t>(val); // C++ handles two's complement automatically for signed cast
}

// Function to convert MSB, LSB to an unsigned 16-bit integer
uint16_t bytesToUnsignedInt16(uint8_t msb, uint8_t lsb) {
    return (static_cast<uint16_t>(msb) << 8) | lsb;
}

// Baud rate mapping
std::map<uint16_t, uint32_t> baudrate_map = {
    {0, 1200},
    {1, 2400},
    {2, 4800},
    {3, 9600},
    {4, 19200},
    {5, 38400},
    {6, 57600}
};

// --------------------------- Serial Port Communication Functions ---------------------------

// Function to configure serial port
int configure_serial_port(const std::string& port_path, speed_t baud_rate_const) {
    int serial_port = open(port_path.c_str(), O_RDWR | O_NOCTTY | O_SYNC); // O_NOCTTY: not controlling terminal, O_SYNC: write synchronously
    if (serial_port < 0) {
        std::cerr << "Error " << errno << " opening serial port " << port_path << ": " << strerror(errno) << std::endl;
        return -1;
    }

    struct termios tty;
    if (tcgetattr(serial_port, &tty) != 0) {
        std::cerr << "Error " << errno << " from tcgetattr: " << strerror(errno) << std::endl;
        close(serial_port);
        return -1;
    }

    cfsetospeed(&tty, baud_rate_const);
    cfsetispeed(&tty, baud_rate_const);

    // 8N1 settings
    tty.c_cflag &= ~PARENB;        // No parity
    tty.c_cflag &= ~CSTOPB;        // 1 stop bit
    tty.c_cflag &= ~CSIZE;         // Clear current character size mask
    tty.c_cflag |= CS8;            // 8 data bits
    tty.c_cflag &= ~CRTSCTS;       // No hardware flow control (RTS/CTS)
    tty.c_cflag |= CREAD | CLOCAL; // Enable reading, ignore modem control lines

    // Local flags
    tty.c_lflag &= ~ICANON; // Disable canonical mode (raw input)
    tty.c_lflag &= ~ECHO;   // Disable echo
    tty.c_lflag &= ~ECHOE;  // Disable erasure
    tty.c_lflag &= ~ECHONL; // Disable new-line echo
    tty.c_lflag &= ~ISIG;   // Disable interpretation of INTR, QUIT and SUSP

    // Input flags
    tty.c_iflag &= ~(IXON | IXOFF | IXANY); // Turn off software flow control
    tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL); // Disable special handling of bytes

    // Output flags
    tty.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes (raw output)
    tty.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed

    // VMIN and VTIME ensure read() blocks until a certain number of bytes are received or a timeout occurs
    tty.c_cc[VMIN] = 0;  // Minimum bytes to read (0 means `read` returns immediately with available bytes)
    tty.c_cc[VTIME] = 10; // Read timeout in 0.1s increments (1s here). Adjust as needed.

    if (tcsetattr(serial_port, TCSANOW, &tty) != 0) {
        std::cerr << "Error " << errno << " from tcgetattr: " << strerror(errno) << std::endl;
        close(serial_port);
        return -1;
    }

    return serial_port;
}

// Function to send Modbus request and receive response
std::vector<uint8_t> send_modbus_request(int serial_fd, const std::vector<uint8_t>& request, int expected_response_len) {
    if (serial_fd < 0) {
        std::cerr << "Serial port not open." << std::endl;
        return {};
    }

    // Clear any pending data in the receive buffer
    tcflush(serial_fd, TCIFLUSH);

    // Send the request
    ssize_t bytes_written = write(serial_fd, request.data(), request.size());
    if (bytes_written != static_cast<ssize_t>(request.size())) {
        std::cerr << "Error writing to serial port: " << strerror(errno) << std::endl;
        return {};
    }
    // std::cout << "Sent: ";
    // for (uint8_t byte : request) {
    //     std::cout << std::setw(2) << std::setfill('0') << std::hex << static_cast<int>(byte) << " ";
    // }
    // std::cout << std::dec << std::endl;

    // Read the response
    std::vector<uint8_t> response_buffer(expected_response_len);
    ssize_t bytes_read = 0;
    size_t total_bytes_read = 0;
    while (total_bytes_read < expected_response_len) {
        bytes_read = read(serial_fd, response_buffer.data() + total_bytes_read, expected_response_len - total_bytes_read);
        if (bytes_read < 0) {
            if (errno == EINTR) continue; // Interrupted system call, try again
            std::cerr << "Error reading from serial port: " << strerror(errno) << std::endl;
            return {};
        } else if (bytes_read == 0) {
            std::cerr << "Read timeout or no data received. Expected " << expected_response_len << " bytes, got " << total_bytes_read << std::endl;
            return {};
        }
        total_bytes_read += bytes_read;
    }

    if (total_bytes_read != static_cast<size_t>(expected_response_len)) {
         std::cerr << "Incomplete response received. Expected " << expected_response_len << " bytes, got " << total_bytes_read << std::endl;
         response_buffer.resize(total_bytes_read); // Trim to actual received size
    }

    return response_buffer;
}

// --------------------------- Modbus Response Parser (adapted from previous example) ---------------------------

void process_modbus_response(const std::vector<uint8_t>& response_bytes, const std::string& description, uint8_t slave_id_expected) {
    if (response_bytes.empty()) {
        std::cout << "No response received for " << description << std::endl;
        return;
    }

    std::cout << "\n--- Processing " << description << " ---" << std::endl;
    std::cout << "Raw response (bytes): ";
    for (uint8_t byte : response_bytes) {
        std::cout << std::setw(2) << std::setfill('0') << std::hex << static_cast<int>(byte) << " ";
    }
    std::cout << std::dec << std::endl;

    if (response_bytes.size() < 5) { // Minimum 5 bytes: slaveID, FC, count, CRC (2 bytes)
        std::cerr << "Error: Response too short (" << response_bytes.size() << " bytes)." << std::endl;
        return;
    }

    uint8_t slave_id_received = response_bytes[0];
    if (slave_id_received != slave_id_expected) {
        std::cerr << "Error: Slave ID mismatch. Expected 0x" << std::hex << static_cast<int>(slave_id_expected)
                  << ", Received 0x" << static_cast<int>(slave_id_received) << std::dec << std::endl;
        return;
    }

    uint8_t function_code = response_bytes[1];
    uint8_t byte_count = response_bytes[2];

    // Modbus Exception Response check (Function code + 0x80)
    // E.g., if request FC was 0x03, exception FC would be 0x83
    if ((function_code & 0x80) == 0x80) { // Check if MSB is set, indicating an exception
        uint8_t exception_code = response_bytes[3];
        std::cerr << "Modbus Exception Received (Function Code 0x" << std::hex << static_cast<int>(function_code)
                  << ", Exception Code 0x" << static_cast<int>(exception_code) << "): ";
        switch (exception_code) {
            case 0x01: std::cerr << "ILLEGAL FUNCTION"; break;
            case 0x02: std::cerr << "ILLEGAL DATA ADDRESS"; break;
            case 0x03: std::cerr << "ILLEGAL DATA VALUE"; break;
            case 0x04: std::cerr << "SLAVE DEVICE FAILURE"; break;
            default: std::cerr << "UNKNOWN EXCEPTION"; break;
        }
        std::cerr << std::dec << std::endl;
        return;
    }


    // CRC16 Check
    std::vector<uint8_t> data_for_crc(response_bytes.begin(), response_bytes.end() - 2);
    uint16_t received_crc = bytesToUnsignedInt16(response_bytes[response_bytes.size() - 1], response_bytes[response_bytes.size() - 2]); // LSB first then MSB
    uint16_t calculated_crc = calculate_modbus_crc(data_for_crc);
    if (received_crc != calculated_crc) {
        std::cerr << "Warning: CRC mismatch! Expected 0x" << std::setw(4) << std::setfill('0') << std::hex << calculated_crc
                  << ", received 0x" << std::setw(4) << std::setfill('0') << received_crc << std::dec << std::endl;
        // Depending on strictness, you might choose to return here
        // return;
    } else {
        std::cout << "CRC check: OK (received 0x" << std::setw(4) << std::setfill('0') << std::hex << received_crc << std::dec << ")" << std::endl;
    }

    // Data starts at index 3 (after slaveID, FC, byte_count)
    if (function_code == 0x03 || function_code == 0x04) { // Read Holding Registers / Read Input Registers
        if (byte_count != (response_bytes.size() - 5)) {
            std::cerr << "Warning: Byte count in response header (" << static_cast<int>(byte_count)
                      << ") does not match actual data length (" << (response_bytes.size() - 5) << ")." << std::endl;
        }

        // Data is always 16-bit register values in pairs of bytes
        for (int i = 0; i < byte_count; i += 2) {
            if (3 + i + 1 >= response_bytes.size() - 2) { // Ensure we don't read past valid data + CRC
                std::cerr << "Error: Incomplete data in response for " << description << std::endl;
                break;
            }
            uint8_t msb = response_bytes[3 + i];
            uint8_t lsb = response_bytes[4 + i];
            uint16_t raw_value_u16 = bytesToUnsignedInt16(msb, lsb);
            int16_t raw_value_s16 = bytesToSignedInt16(msb, lsb); // Auto handles two's complement

            if (description.find("Temperature") != std::string::npos && description.find("Alarm") == std::string::npos) {
                // Temperature conversion
                float temperature = static_cast<float>(raw_value_s16) / 10.0f;
                std::cout << "Interpreted Temperature: " << std::fixed << std::setprecision(1) << temperature << " °C" << std::endl;
            } else if (description.find("Humidity") != std::string::npos && description.find("Alarm") == std::string::npos) {
                // Humidity conversion
                float humidity = static_cast<float>(raw_value_u16) / 10.0f;
                // Note: Humidity is typically unsigned, so using raw_value_u16
                std::cout << "Interpreted Humidity: " << std::fixed << std::setprecision(1) << humidity << " %RH" << std::endl;
            } else if (description.find("Alarm Status") != std::string::npos) {
                // Alarm status is typically in the LSB of the 16-bit word, as 0x00XX
                uint8_t alarm_code = lsb; // Or raw_value_u16 & 0xFF;
                std::string status_text;
                switch (alarm_code) {
                    case 0x00: status_text = "No Alarm"; break;
                    case 0x01: status_text = "Upper Limit Alarm"; break;
                    case 0x02: status_text = "Lower Limit Alarm"; break;
                    default: status_text = "Unknown Alarm Status"; break;
                }
                std::cout << "Interpreted Alarm Status (Code 0x" << std::hex << static_cast<int>(alarm_code) << std::dec << "): " << status_text << std::endl;
            } else if (description.find("Address Register") != std::string::npos) {
                // Modbus address is a direct unsigned 16-bit value
                std::cout << "Interpreted Address Register Value: 0x" << std::hex << raw_value_u16 << std::dec << " (" << raw_value_u16 << ")" << std::endl;
            } else if (description.find("Baudrate Register") != std::string::npos) {
                // Baudrate is a code mapping
                std::cout << "Baudrate Register Code: " << raw_value_u16;
                if (baudrate_map.count(raw_value_u16)) {
                    std::cout << " (Value: " << baudrate_map[raw_value_u16] << " bps)";
                } else {
                    std::cout << " (Warning: Unknown baudrate code)";
                }
                std::cout << std::endl;
            } else {
                std::cout << "Raw Register Value (Hex): 0x" << std::hex << raw_value_u16 << std::dec << std::endl;
                std::cout << "Raw Register Value (Decimal): " << raw_value_u16 << std::endl;
            }
        }
    } else {
        std::cout << "Unsupported/Unknown Function Code: 0x" << std::hex << static_cast<int>(function_code) << std::dec << std::endl;
    }
}

// --------------------------- Main Program ---------------------------

int main() {
    std::cout << "--- JWST-20 Sensor Real-Time Test (Corrected Interpretation Logic) ---" << std::endl;

    const std::string serial_port_path = "/dev/ttyS7"; // <-- !!! Replace with your actual serial port !!!
    const speed_t modbus_baud_rate = B9600;            // Modbus default baud rate (9600 bps)
    const uint8_t slave_id = 0x01;                      // Sensor's Modbus slave ID

    int serial_fd = configure_serial_port(serial_port_path, modbus_baud_rate);
    if (serial_fd < 0) {
        return 1; // Exit on serial port error
    }

    // --- Modbus Request Definitions (from your datasheet/original test program) ---
    // Address: 0x0000 (0), Quantity: 1 register (temperature)
    std::vector<uint8_t> temp_request = {
        slave_id, 0x03, // Slave ID, Function Code (Read Holding Registers)
        0x00, 0x00, // Starting Address High, Low (0x0000)
        0x00, 0x01  // Quantity of Registers High, Low (1 register)
    };
    uint16_t temp_crc = calculate_modbus_crc(temp_request);
    temp_request.push_back(temp_crc & 0xFF); // CRC Low byte
    temp_request.push_back((temp_crc >> 8) & 0xFF); // CRC High byte
    // Expected response length: SlaveID (1) + FC (1) + ByteCount (1) + Data (2*1) + CRC (2) = 7 bytes


    // Address: 0x0001 (1), Quantity: 1 register (humidity)
    std::vector<uint8_t> humidity_request = {
        slave_id, 0x03, // Slave ID, Function Code (Read Holding Registers)
        0x00, 0x01, // Starting Address High, Low (0x0001)
        0x00, 0x01  // Quantity of Registers High, Low (1 register)
    };
    uint16_t humidity_crc = calculate_modbus_crc(humidity_request);
    humidity_request.push_back(humidity_crc & 0xFF);
    humidity_request.push_back((humidity_crc >> 8) & 0xFF);


    // Address: 0x0000 (0), Quantity: 1 register (Temperature Alarm, Input Register)
    std::vector<uint8_t> temp_alarm_request = {
        slave_id, 0x04, // Slave ID, Function Code (Read Input Registers)
        0x00, 0x00, // Starting Address High, Low (0x0000)
        0x00, 0x01  // Quantity of Registers High, Low (1 register)
    };
    uint16_t temp_alarm_crc = calculate_modbus_crc(temp_alarm_request);
    temp_alarm_request.push_back(temp_alarm_crc & 0xFF);
    temp_alarm_request.push_back((temp_alarm_crc >> 8) & 0xFF);


    // Address: 0x0001 (1), Quantity: 1 register (Humidity Alarm, Input Register)
    std::vector<uint8_t> humidity_alarm_request = {
        slave_id, 0x04, // Slave ID, Function Code (Read Input Registers)
        0x00, 0x01, // Starting Address High, Low (0x0001)
        0x00, 0x01  // Quantity of Registers High, Low (1 register)
    };
    uint16_t humidity_alarm_crc = calculate_modbus_crc(humidity_alarm_request);
    humidity_alarm_request.push_back(humidity_alarm_crc & 0xFF);
    humidity_alarm_request.push_back((humidity_alarm_crc >> 8) & 0xFF);


    // Address: 0x1000 (4096), Quantity: 1 register (Modbus Address)
    std::vector<uint8_t> modbus_addr_request = {
        slave_id, 0x03,
        0x10, 0x00, // Starting Address (0x1000)
        0x00, 0x01
    };
    uint16_t modbus_addr_crc = calculate_modbus_crc(modbus_addr_request);
    modbus_addr_request.push_back(modbus_addr_crc & 0xFF);
    modbus_addr_request.push_back((modbus_addr_crc >> 8) & 0xFF);


    // Address: 0x1001 (4097), Quantity: 1 register (Baudrate)
    std::vector<uint8_t> baudrate_request = {
        slave_id, 0x03,
        0x10, 0x01, // Starting Address (0x1001)
        0x00, 0x01
    };
    uint16_t baudrate_crc = calculate_modbus_crc(baudrate_request);
    baudrate_request.push_back(baudrate_crc & 0xFF);
    baudrate_request.push_back((baudrate_crc >> 8) & 0xFF);


    // --- Main Loop: Read and Process Data ---
    std::cout << "\nStarting continuous Modbus polling. Press Ctrl+C to exit." << std::endl;

    while (true) {
        std::vector<uint8_t> response;

        // 1. Read Temperature
        response = send_modbus_request(serial_fd, temp_request, 7); // Expected 7 bytes: 1+1+1+2*1+2
        process_modbus_response(response, "Live Temperature", slave_id);

        // 2. Read Humidity
        response = send_modbus_request(serial_fd, humidity_request, 7);
        process_modbus_response(response, "Live Humidity", slave_id);

        // 3. Read Temperature Alarm Status (using Function Code 0x04 for Input Registers)
        response = send_modbus_request(serial_fd, temp_alarm_request, 7);
        process_modbus_response(response, "Live Temperature Alarm Status (Input Register)", slave_id);

        // 4. Read Humidity Alarm Status (using Function Code 0x04 for Input Registers)
        response = send_modbus_request(serial_fd, humidity_alarm_request, 7);
        process_modbus_response(response, "Live Humidity Alarm Status (Input Register)", slave_id);

        // 5. Read Modbus Address
        response = send_modbus_request(serial_fd, modbus_addr_request, 7);
        process_modbus_response(response, "Live Modbus Address Register", slave_id);

        // 6. Read Baudrate
        response = send_modbus_request(serial_fd, baudrate_request, 7);
        process_modbus_response(response, "Live Baudrate Register", slave_id);

        sleep(2); // Wait 2 seconds before polling again to avoid flooding the sensor
    }

    close(serial_fd); // Close the serial port when done
    std::cout << "\nProgram terminated." << std::endl;
    return 0;
}