#include <stdio.h>
#include "pico/stdlib.h"
#include "ble_server.h"
#include "mpu_handler.h"
#include "gps_handler.h"

#define GENERIC_MESSAGE_SEND_INTERVAL_MS 100

static repeating_timer_t timer;
static char ble_buffer[64];

static MPU_Data_t mpu_values;
static GPS_Data_t gps_values;

bool periodic_message_timer_callback(repeating_timer_t *rt) {
    (void)rt;

    // Data Format: S,AccX,AccY,AccZ,Lat,Lon,Fix
    snprintf(ble_buffer, sizeof(ble_buffer),
             "S,%.2f,%.2f,%.2f,%.4f,%.4f,%d",
             mpu_values.ax, mpu_values.ay, mpu_values.az,
             gps_values.latitude, gps_values.longitude,
             gps_values.has_fix ? 1 : 0);

    send_message(ble_buffer);

    return true;
}

int main() {
    stdio_init_all();
    sleep_ms(1000);

    start_server();
    mpu_init();
    mpu_calibrate();
    gps_init();

    if (!add_repeating_timer_ms(GENERIC_MESSAGE_SEND_INTERVAL_MS, periodic_message_timer_callback, NULL, &timer)) {
        printf("Failed to start timer!\n");
    }

    while (true) {
        mpu_read_data(&mpu_values);
        gps_update(&gps_values);
        sleep_ms(50);
    }

    cancel_repeating_timer(&timer);
    stop_server();
    return 0;
}