#ifndef FUSION_BUS_H
#define FUSION_BUS_H

#include <stdbool.h>
#include <stdint.h>

#include "fusion_measurement.h"

#ifndef FUSION_IMU_QUEUE_CAPACITY
#define FUSION_IMU_QUEUE_CAPACITY 256u
#endif

#ifndef FUSION_GPS_QUEUE_CAPACITY
#define FUSION_GPS_QUEUE_CAPACITY 8u
#endif

typedef struct {
	fusion_imu_measurement_t imu_queue[FUSION_IMU_QUEUE_CAPACITY];
	fusion_gps_measurement_t gps_queue[FUSION_GPS_QUEUE_CAPACITY];
	volatile unsigned int imu_head;
	volatile unsigned int imu_tail;
	volatile unsigned int gps_head;
	volatile unsigned int gps_tail;
	volatile uint64_t imu_overflow_count;
	volatile uint64_t gps_overflow_count;
} fusion_bus_t;

void fusion_bus_init(fusion_bus_t* bus);

/* Single producer (TDK ISR), single consumer (foreground). */
bool fusion_bus_publish_imu(fusion_bus_t* bus,
	                        const fusion_imu_measurement_t* measurement);
bool fusion_bus_take_imu(fusion_bus_t* bus,
	                     fusion_imu_measurement_t* measurement_out);

/* Single producer (GPS WLS foreground), single fusion consumer. */
bool fusion_bus_publish_gps(fusion_bus_t* bus,
	                        const fusion_gps_measurement_t* measurement);
bool fusion_bus_take_gps(fusion_bus_t* bus,
	                     fusion_gps_measurement_t* measurement_out);

#endif