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

#include <arpa/inet.h>
#include <fcntl.h>
#include <sys/socket.h>
#include <sys/fcntl.h> 
#include <sys/stat.h>
#include <sys/ioctl.h> 
#include <linux/i2c-dev.h>

// #include <px4_platform_common/px4_config.h>
// #include <px4_platform_common/log.h>
// #include <px4_platform_common/tasks.h>
// #include <drivers/drv_hrt.h>
#define I2C_SLAVE 0x0703  // Dummy value for testing

#define UDP_PORT 5000

// extern "C" __EXPORT int hmc5883l_main(int argc, char *argv[]);

int mag_i2c_file;
int mag_udp_socket;
struct sockaddr_in mag_destAddr;

void magnetometer_init();

void magnetometer_spg() 
{

    // Initialize HMC5883L
    magnetometer_init();
       
    // Sleep is not necessary, will be done by AADL
    // px4_usleep(1000);
    // sleep(1);

    // Read 6 bytes of data from register(0x03)
    // xMag msb, xMag lsb, zMag msb, zMag lsb, yMag msb, yMag lsb
    char reg = 0x03;
    char data[6] = {0};
    // uint8_t reg = 0x03;
    // uint8_t data[6] = {0};
    if (write(mag_i2c_file, &reg, 1) != 1 || read(mag_i2c_file, data, 6) != 6) {
        perror("Error reading from the I2C device");
        // PX4_ERR("Error reading from the I2C device");
        // return -1;
        exit(1);
    }

    // Convert the data
    // int16_t xMag
    int xMag = (data[0] * 256 + data[1]);
    if (xMag > 32767) {
        xMag -= 65536;
    }
	
    // int16_t zMag
    int zMag = (data[2] * 256 + data[3]);
    if (zMag > 32767) {
        zMag -= 65536;
    }
    
    // int16_t yMag
    int yMag = (data[4] * 256 + data[5]);
    if (yMag > 32767) {
        yMag -= 65536;
    }

    // Format the sensor data as a string
    char sensorData[50];
    snprintf(sensorData, sizeof(sensorData), "X:%d, Y:%d, Z:%d", xMag, yMag, zMag);

    // Send the sensor data over UDP
    if (sendto(mag_udp_socket, sensorData, strlen(sensorData), 0, (struct sockaddr*)&mag_destAddr, sizeof(mag_destAddr)) < 0) {
        perror("Error sending data");
        // PX4_ERR("Error sending data");
        // return -1;
        exit(1);
    }

    // Close the socket and I2C file
    close(mag_udp_socket);
    close(mag_i2c_file);
}

void magnetometer_init()
{
    // Create socket
    if ((mag_udp_socket = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
        perror("Error creating the socket!");
        // PX4_ERR("Error creating socket!");
        // return -1;
        exit(1);
    }

    // Set destination address
    memset(&mag_destAddr, 0, sizeof(mag_destAddr));
    mag_destAddr.sin_family = AF_INET;
    mag_destAddr.sin_port = htons(UDP_PORT);
    if (inet_pton(AF_INET, "PIXHAWK_IP_ADDRESS", &(mag_destAddr.sin_addr)) <= 0) // Replace with real address
    {
        perror("Error connecting to the address!");
        // PX4_ERR("Error setting address!");
        // return -1;
        exit(1);
    }

    // Create I2C bus
    char *bus = "/dev/i2c-1"; // Check this path!
    if ((mag_i2c_file = open(bus, O_RDWR)) < 0) 
    {
        perror("Error creating the bus!");
        // PX4_ERR("Error creating the bus!");
        // return -1;
        exit(1);
    }

    // Get I2C device, HMC5883 I2C address is 0x1E(30)
    if (ioctl(mag_i2c_file, I2C_SLAVE, 0x1E) < 0) 
    {
        perror("Error getting the address!");
        // PX4_ERR("Error getting the address!");
        // return -1;
        exit(1);
    }

    // Select Configuration register A(0x00)
    // Normal measurement configuration, data rate o/p = 0.75 Hz(0x60)
    char config[2] = {0x00, 0x60};
    if (write(mag_i2c_file, config, 2) != 2) 
    {
        perror("Error writing to the I2C device!");
        // PX4_ERR("Error writing to the I2C device!");
        // return -1;
        exit(1);
    }

    // Select Mode register(0x02)
    // Continuous measurement mode(0x00)
    config[0] = 0x02;
    config[1] = 0x00;
    if (write(mag_i2c_file, config, 2) != 2) 
    {
        perror("Error writing to the I2C device!");
        // PX4_ERR("Error writing to the I2C device!");
        // return -1;
        exit(1);
    }
}