#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/drivers/sensor.h>
#include <zephyr/drivers/uart.h>
#include <zephyr/drivers/gnss.h>
#include <zephyr/logging/log.h>
#include <stdio.h>
#include <zephyr/sys/util.h>

// static int print_samples;
// static int lsm6dsl_trig_cnt;

// static struct sensor_value accel_x_out, accel_y_out, accel_z_out;
// static struct sensor_value gyro_x_out, gyro_y_out, gyro_z_out;
// // const struct device *uart = DEVICE_DT_GET(DT_NODELABEL(uart0)); 

// #ifdef CONFIG_LSM6DSL_TRIGGER
// static void lsm6dsl_trigger_handler(const struct device *dev,
// 				    const struct sensor_trigger *trig)
// {
// 	static struct sensor_value accel_x, accel_y, accel_z;
// 	static struct sensor_value gyro_x, gyro_y, gyro_z;
// 	lsm6dsl_trig_cnt++;

// 	sensor_sample_fetch_chan(dev, SENSOR_CHAN_ACCEL_XYZ);
// 	sensor_channel_get(dev, SENSOR_CHAN_ACCEL_X, &accel_x);
// 	sensor_channel_get(dev, SENSOR_CHAN_ACCEL_Y, &accel_y);
// 	sensor_channel_get(dev, SENSOR_CHAN_ACCEL_Z, &accel_z);

// 	/* lsm6dsl gyro */
// 	sensor_sample_fetch_chan(dev, SENSOR_CHAN_GYRO_XYZ);
// 	sensor_channel_get(dev, SENSOR_CHAN_GYRO_X, &gyro_x);
// 	sensor_channel_get(dev, SENSOR_CHAN_GYRO_Y, &gyro_y);
// 	sensor_channel_get(dev, SENSOR_CHAN_GYRO_Z, &gyro_z);

// 	if (print_samples) {
// 		print_samples = 0;

// 		accel_x_out = accel_x;
// 		accel_y_out = accel_y;
// 		accel_z_out = accel_z;

// 		gyro_x_out = gyro_x;
// 		gyro_y_out = gyro_y;
// 		gyro_z_out = gyro_z;
// 	}

// }
// #endif

const struct device *gps_dev = DEVICE_DT_GET(DT_NODELABEL(uart0));

void gps_uart_callback(const struct device *dev, void *user_data) {
    uint8_t byte;
    if (!uart_irq_update(dev)) return;

    while (uart_irq_rx_ready(dev)) {
        if (uart_fifo_read(dev, &byte, 1) > 0) {
            printk("%c", byte); 
        }
    }
}
int main(void)
{

	if (!device_is_ready(gps_dev)) {
        printk("UART device not ready\n");
        return 0;
    }

	uart_irq_callback_user_data_set(gps_dev, gps_uart_callback, NULL);
    uart_irq_rx_enable(gps_dev);

// 	int cnt = 0;
// 	char out_str[64];
// 	struct sensor_value odr_attr;
// 	const struct device *const lsm6dsl_dev = DEVICE_DT_GET_ONE(st_lsm6dsl);

// 	if (!device_is_ready(lsm6dsl_dev)) {
// 		printk("sensor: device not ready.\n");
// 		return 0;
// 	}

// 	/* set accel/gyro sampling frequency to 104 Hz */
// 	odr_attr.val1 = 104;
// 	odr_attr.val2 = 0;

// 	if (sensor_attr_set(lsm6dsl_dev, SENSOR_CHAN_ACCEL_XYZ,
// 			    SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
// 		printk("Cannot set sampling frequency for accelerometer.\n");
// 		return 0;
// 	}

// 	if (sensor_attr_set(lsm6dsl_dev, SENSOR_CHAN_GYRO_XYZ,
// 			    SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
// 		printk("Cannot set sampling frequency for gyro.\n");
// 		return 0;
// 	}

// #ifdef CONFIG_LSM6DSL_TRIGGER
// 	struct sensor_trigger trig;

// 	trig.type = SENSOR_TRIG_DATA_READY;
// 	trig.chan = SENSOR_CHAN_ACCEL_XYZ;

// 	if (sensor_trigger_set(lsm6dsl_dev, &trig, lsm6dsl_trigger_handler) != 0) {
// 		printk("Could not set sensor type and channel\n");
// 		return 0;
// 	}
// #endif



	// if (sensor_sample_fetch(lsm6dsl_dev) < 0) {
	// 	printk("Sensor sample update error\n");
	// 	return 0;
	// }

	while (1) {
		// /* Erase previous */
		// printk("\0033\014");
		// printf("LSM6DSL sensor samples:\n\n");

		// /* lsm6dsl accel */
		// sprintf(out_str, "accel x:%f ms/2 y:%f ms/2 z:%f ms/2",
		// 					  sensor_value_to_double(&accel_x_out),
		// 					  sensor_value_to_double(&accel_y_out),
		// 					  sensor_value_to_double(&accel_z_out));
		// printk("%s\n", out_str);

        // sprintf(out_str, "G-X: %d G-Y: %d G-Z: %d",
        //                     sensor_ms2_to_g(&accel_x_out),
        //                     sensor_ms2_to_g(&accel_y_out),
        //                     sensor_ms2_to_g(&accel_z_out));
        // printk("%s\n", out_str);

		// /* lsm6dsl gyro */
		// sprintf(out_str, "gyro x:%f dps y:%f dps z:%f dps",
		// 					   sensor_value_to_double(&gyro_x_out),
		// 					   sensor_value_to_double(&gyro_y_out),
		// 					   sensor_value_to_double(&gyro_z_out));
		// printk("%s\n", out_str);

		// printk("loop:%d trig_cnt:%d\n\n", ++cnt, lsm6dsl_trig_cnt);

		// print_samples = 1;
		k_sleep(K_MSEC(200));

		k_sleep(K_SECONDS(2));
	}
}