//DHT22 SENSOR
#include <stdint.h>
#include <stdio.h> 
#include <unistd.h>
#include <string.h>
#include <stdlib.h>

#include <arpa/inet.h>
#include <sys/socket.h>
// #include <gpiod.h>
#include <po_hi_time.h>

// Define pin for connecting the data wire of the DHT sensor
#define DHT_PIN 12

#define SERVER_IP "192.168.0.100" // IP address of the Pixhawk
#define SERVER_PORT 5000 // Port number on which Pixhawk is listening

int tube_udp_socket;
struct sockaddr_in tube_destAddr;

void tube_init();
// void readData(int* temperature, int* humidity);

void tube_sensor_spg() {

    tube_init();

    // Read temperature and humidity from DHT sensor
    int temperature, humidity;
    // readData(&temperature, &humidity); // gcc -o your_program pressure.c -lgpiod !!!IMPORTANT!!!

    float floatTemperature = temperature * 0.1;
    float floatHumidity = humidity * 0.1;

    // Print temperature and humidity values to screen
    // printf("Temperature = %.1f Humidity = %.1f\n", floatTemperature, floatHumidity);

    // Prepare the sensor data packet
    char dataPacket[64]; // Adjust the packet size as needed
    sprintf(dataPacket, "Temperature = %.1f Humidity = %.1f", floatTemperature, floatHumidity);

    // Send the data packet to Pixhawk
    if (send(tube_udp_socket, dataPacket, strlen(dataPacket), 0) < 0) 
    {
        perror("Error sending data to Pixhawk!");
        exit(1);
    }

    close(tube_udp_socket);
}

void tube_init() 
{
    // Create socket
    tube_udp_socket = socket(AF_INET, SOCK_DGRAM, 0);
    if (tube_udp_socket < 0) 
    {
        perror("Error creating the socket!");
        exit(1);
    }

    // Set server address
    memset(&tube_destAddr, 0, sizeof(tube_destAddr));
    tube_destAddr.sin_family = AF_INET;
    tube_destAddr.sin_port = htons(SERVER_PORT); // Replace SERVER_PORT with the actual port number
    if (inet_aton(SERVER_IP, &tube_destAddr.sin_addr) == 0) 
    {
        perror("Error setting the address!");
        exit(1);
    }

    // Connect to the server
    if (connect(tube_udp_socket, (struct sockaddr*)&tube_destAddr, sizeof(tube_destAddr)) < 0) 
    {
        perror("Error connecting to the server!");
        exit(1);
    }
}

// IMPORTANT!!! I used libgpiod library, so it has to be installed first: sudo apt-get install libgpiod-dev
/*void readData(int* temperature, int* humidity) {
    struct gpiod_chip *chip;
    struct gpiod_line *line;

    // Initialize GPIO library
    chip = gpiod_chip_open("/dev/gpiochip0"); // Change if necessary
    if (!chip) {
        perror("Failed to open GPIO chip!");
        exit(1);
    }

    // Set pin direction to output
    line = gpiod_chip_get_line(chip, DHT_PIN);
    if (!line) {
        perror("Failed to get GPIO line!");
        exit(1);
    }
    if (gpiod_line_request_output(line, "DHT22", GPIOD_LINE_ACTIVE_STATE_LOW) < 0) { // Change if necessary
        perror("Failed to set GPIO line direction!");
        exit(1);
    }

    // Send start signal to the sensor
    gpiod_line_set_value(line, 0);
    usleep(18000); // Wait for at least 18 milliseconds

    // Set pin direction to input
    if (gpiod_line_request_input(line, "DHT22") < 0) {
        perror("Failed to set GPIO line direction!");
        exit(1);
    }

    // Wait for sensor response
    int response = 0;
    while (gpiod_line_get_value(line) == 1) {
        usleep(1);
        response++;
        if (response > 100) {
            perror("Sensor failed to respond!");
            exit(1);
        }
    }
    
    // Read data bits from the sensor
    uint8_t data[5] = { 0 };
    for (int i = 0; i < 5; i++) {
        for (int j = 0; j < 8; j++) {
            while (gpiod_line_get_value(line) == 0);    // Wait for the start of the data bit
            usleep(30);  // Delay to determine the value of the data bit
            if (gpiod_line_get_value(line) == 1)
                data[i] |= (1 << (7 - j));   // Set the corresponding bit in the data array
            while (gpiod_line_get_value(line) == 1);   // Wait for the end of the data bit
        }
    }

    // Check data integrity by verifying the checksum
    uint8_t checksum = data[0] + data[1] + data[2] + data[3];
    if (checksum != data[4]) {
        perror("Checksum verification failed!");
        exit(1);
    }

    // Calculate temperature and humidity values
    *temperature = (int16_t)((data[2] << 8) | data[3]); // temperature = (int16_t)(data[2] * 256 + data[3]);
    *humidity = (int16_t)((data[0] << 8) | data[1]); // Necessary because of the DHT data format

    // Cleanup GPIO resources
    gpiod_chip_close(chip);
}*/