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

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

// Define pin for connecting the data wire of the pressure sensor
#define PRESSURE_PIN 12

#define SERVER_PORT 1234  // Replace with the actual port number
#define SERVER_IP_ADDRESS "127.0.0.1"  // Replace with the actual IP address

int pressure_udp_socket;
struct sockaddr_in pressure_destAddr;

void pressure_init();
// int analogRead(int);
// int readPressure();

void pressure_spg()
{
    pressure_init();

    // Read pressure from the SKU237545 pressure sensor
    int pressure = 0;
    // pressure = readPressure(); // gcc -o your_program pressure.c -lgpiod !!!!IMPORTANT!!!

    // Print pressure value to the screen
    // printf("Pressure = %d\n", pressure);

    // Prepare the sensor data packet
    char dataPacket[64]; // Adjust the packet size as needed
    sprintf(dataPacket, "Pressure = %d", pressure);

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

    close(pressure_udp_socket);
}

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

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

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

// Function to read analog value from a pin
/*int readPressure()
{
    // Initialize GPIO library
    struct gpiod_chip* chip = gpiod_chip_open("/dev/gpiochip0"); // Change if necessary
    if (!chip) {
        perror("Failed to open GPIO chip!");
        exit(1);
    }

    // Set pin direction to output
    struct gpiod_line* line = gpiod_chip_get_line(chip, PRESSURE_PIN);
    if (!line) {
        perror("Failed to get GPIO line!");
        exit(1);
    }
    if (gpiod_line_request_output(line, "PressureSensor", GPIOD_LINE_ACTIVE_STATE_DEFAULT) < 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(2000); // Wait for 2 milliseconds

    // Set pin direction to input
    if (gpiod_line_request_input(line, "PressureSensor") < 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 analog value from the sensor
    int analogValue = analogRead(PRESSURE_PIN); // Replace with the appropriate function to read analog value

    // Calculate pressure value based on analog reading
    // Assuming offset = 94 and maxReading = 920 (as written in documentation)
    float pressure = ((analogValue - 94) * 1.2) / (920 - 94);

    // Cleanup GPIO resources
    gpiod_chip_close(chip);

    return (int)(pressure * 1000); // Return pressure in millipascals (mPa)
}

// Function to read analog value from a pin
int analogRead(int pin) {
    // TODO: ADC conversion
    int analogValue = 512;




    return analogValue;
}*/