// LUMEN SUBSEA LIGHTS
#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>

#define SERVER_IP "192.168.0.100" // IP address of the Pixhawk
#define SERVER_PORT 5000 // Port number on which Pixhawk is listening
#define OUTPUT 1 // voltage/ground
#define HIGH 1 // 5V or logical 1
#define LOW 0

int ledPin1 = 33;
int ledPin2 = 35;
int lights_udp_socket;
struct sockaddr_in server_address;
struct gpiod_chip* chip;
struct gpiod_line* line;

void lights_init();
void update_led_state(int value);
void lightsPinMode(int pin, int mode);
void lightsDigitalWrite(int pin, int value);

void lights_spg()
{
    lights_init();

    // Read LED control value from Pixhawk
    // ...

    int ledControlValue = 1111; // Replace with the value received from Pixhawk

    // Perform LED control based on the received value
    update_led_state(ledControlValue);

    // Send the LED control value back to Pixhawk
    sendto(lights_udp_socket, &ledControlValue, sizeof(ledControlValue), 0, (struct sockaddr*) &server_address, sizeof(server_address));

    close(lights_udp_socket);
}

void lights_init()
{
    // Create socket
    lights_udp_socket = socket(AF_INET, SOCK_DGRAM, 0);
    if (lights_udp_socket == -1) {
        perror("Socket creation failed!");
        exit(-1);
    }

    // Set server address
    memset(&server_address, 0, sizeof(server_address));
    server_address.sin_family = AF_INET;
    server_address.sin_port = htons(SERVER_PORT);
    if (inet_pton(AF_INET, SERVER_IP, &(server_address.sin_addr)) <= 0) 
    {
        perror("Invalid address!");
        exit(-1);
    }

    // Initialize LED pins for GPIO?
    // ...

    // Initialize LED pins - commented because of the library used
    // lightsPinMode(ledPin1, OUTPUT);
    // lightsPinMode(ledPin2, OUTPUT);
    // lightsDigitalWrite(ledPin1, HIGH);
    // lightsDigitalWrite(ledPin2, HIGH);
}

void update_led_state(int value)
{
    switch (value) // commented because of the library used
    {
        case 1234: // defined as such in the original code
            // lightsDigitalWrite(ledPin1, LOW);
            // lightsDigitalWrite(ledPin2, LOW);
            break;
        case 1111: // defined as such in the original code
            // lightsDigitalWrite(ledPin1, HIGH);
            // lightsDigitalWrite(ledPin2, HIGH);
            break;
        // Add more cases if needed

        default:
            // Handle unsupported values or errors
            break;
    }
}

// IMPORTANT!!! I used libgpiod library, so it has to be installed first: sudo apt-get install libgpiod-dev
/*void lightsPinMode(int pin, int mode) {
    int temp;

    // Open the GPIO chip
    chip = gpiod_chip_open("/dev/gpiochip0");
    if (!chip) {
        perror("Failure opening GPIO chip!");
        exit(-1);
    }

    // Get the GPIO line
    line = gpiod_chip_get_line(chip, pin);
    if (!line) {
        perror("Failure getting GPIO line!");
        exit(-1);
    }

    // Set the pin direction
    if (mode == OUTPUT) {
        temp = gpiod_line_request_output(line, "my-output", 0);
    } else {
        temp = gpiod_line_request_input(line, "my-input");
    }

    if (temp < 0) {
        perror("Failure setting pin mode!");
        exit(-1);
    }
}

void lightsDigitalWrite(int pin, int value) 
{
    // Set the pin state
    int temp;
    if (value == HIGH) {
        temp = gpiod_line_set_value(line, 1);
    } else {
        temp = gpiod_line_set_value(line, 0);
    }

    if (temp < 0) {
        perror("Failure setting pin value!");
        exit(-1);
    }
}*/