// BLUE ROBOTICS PSM
#include <stdint.h>
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>

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

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

int power_udp_socket;
struct sockaddr_in power_serverAddr;

void power_sensing_module_init();
void readPowerData(float *current, float *voltage);

void power_sensing_module_spg()
{
    power_sensing_module_init();

    // Read power data
    float current = 0.0, voltage = 0.0;
    readPowerData(&current, &voltage);

    printf("Current: %.2f A\n", current);
    printf("Voltage: %.2f V\n", voltage);

    // Send power data to Pixhawk
    char powerStr[50]; // Modify the size as per your data requirements
    sprintf(powerStr, "Current: %.2f A, Voltage: %.2f V", current, voltage);

    if (send(power_udp_socket, powerStr, strlen(powerStr), 0) == -1)
        perror("Error sending the data!");

    close(power_udp_socket);
}

void power_sensing_module_init()
{
    // Create socket
    if ((power_udp_socket = socket(AF_INET, SOCK_DGRAM, 0)) == -1)
    { // For TCP, SOCK_STREAM
        perror("socket");
        exit(1);
    }

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

    // Connect to the server
    if (connect(power_udp_socket, (struct sockaddr *)&power_serverAddr, sizeof(power_serverAddr)) == -1)
    {
        perror("connect");
        exit(1);
    }
}

void readPowerData(float *current, float *voltage)
{
    // Replace the code below with implementation to read the current and voltage values
    // GPIO, maybe?
    *current = 1.23;
    *voltage = 12.34;
}