1 files changed, 27 insertions, 7 deletions

diff --git a/src/ufo-roof-buffer.c b/src/ufo-roof-buffer.c
index 179d153..bac940c 100644
--- a/src/ufo-roof-buffer.c
+++ b/src/ufo-roof-buffer.c
@@ -9,15 +9,24 @@
 
 // This is currently not thread safe. With dual-filter architecture this will be called sequentially.
 
-UfoRoofBuffer *ufo_roof_buffer_new(UfoRoofConfig *cfg, guint max_datasets, GError **error) {
+UfoRoofBuffer *ufo_roof_buffer_new(UfoRoofConfig *cfg, guint n_dims, guint max_datasets, GError **error) {
+    if ((n_dims < 1)||(n_dims > 2))
+	roof_new_error(error, "Unsupported number of dimmensions %u (only plain and 2D ROOF structure is currently supported)", n_dims);
+
     UfoRoofBuffer *buffer = (UfoRoofBuffer*)calloc(1, sizeof(UfoRoofBuffer));
     if (!buffer) roof_new_error(error, "Can't allocate UfoRoofBuffer");
 
     buffer->max_datasets = max_datasets;
     buffer->ring_size = cfg->buffer_size;
     buffer->drop_buffers = cfg->drop_buffers;
-    buffer->fragment_size = cfg->payload_size;
+    buffer->n_dims = n_dims;
     buffer->dataset_size = cfg->dataset_size;
+    buffer->dataset_dims[0] = cfg->fan_bins * cfg->bit_depth / 8;
+    buffer->dataset_dims[1] = cfg->fan_projections;
+    buffer->fragment_size = cfg->payload_size;
+    buffer->fragment_dims[0] = cfg->channels_per_module * cfg->bit_depth / 8;
+    buffer->fragment_dims[1] = buffer->fragment_size / buffer->fragment_dims[0];
+
     buffer->fragments_per_dataset = buffer->dataset_size / buffer->fragment_size;
     buffer->fragments_per_stream = buffer->fragments_per_dataset / cfg->n_streams;
 //    printf("Configuration: dataset: %u - %u fragments (%u streams x %u) x %u bytes\n", buffer->dataset_size, buffer->fragments_per_dataset, cfg->n_streams, buffer->fragments_per_stream, buffer->fragment_size);
@@ -96,15 +105,25 @@ gboolean ufo_roof_buffer_set_fragment(UfoRoofBuffer *buffer, guint stream_id, gu
             // The updates may happen after writting/reading is finished.
     }
 
-        // FIXME: This is builds events as it read from file in roof v.1 code. We can assemble fan projections directly here.
-    uint8_t *dataset_buffer = buffer->ring_buffer + buffer_id * buffer->dataset_size;
-    uint8_t *fragment_buffer = dataset_buffer + (stream_id * buffer->fragments_per_stream + fragment_id) * buffer->fragment_size;
-    
 /*    printf("buffer: %u (%u), packet: %u (%ux%u %u), packet_size: %u [%x]\n", 
         buffer_id, dataset_id, stream_id * buffer->fragments_per_stream + fragment_id, stream_id, buffer->fragments_per_stream, fragment_id, buffer->fragment_size,
         ((uint32_t*)fragment)[0]
     );*/
-    memcpy(fragment_buffer, fragment, buffer->fragment_size);
+
+    uint8_t *dataset_buffer = buffer->ring_buffer + buffer_id * buffer->dataset_size;
+    if (buffer->n_dims == 2) {
+	uint8_t *fragment_buffer = dataset_buffer + 
+		stream_id * buffer->fragment_dims[0] +					// x-coordinate
+		(fragment_id * buffer->fragment_dims[1]) * buffer->dataset_dims[0];	// y-coordinate
+
+	for (int i = 0; i < buffer->fragment_dims[1]; ++i) {
+	    memcpy(fragment_buffer + i * buffer->dataset_dims[0], fragment + i * buffer->fragment_dims[0], buffer->fragment_dims[0]);
+	}
+    } else {
+	    // 1D stracture, simply putting fragment at the appropriate position in the stream
+	uint8_t *fragment_buffer = dataset_buffer + (stream_id * buffer->fragments_per_stream + fragment_id) * buffer->fragment_size;
+	memcpy(fragment_buffer, fragment, buffer->fragment_size);
+    }
 
         // FIXME: Sanity checks: verify is not a dublicate fragment?
     atomic_fetch_add(&buffer->n_fragments[buffer_id], 1);
@@ -128,6 +147,7 @@ gboolean ufo_roof_buffer_get_dataset(UfoRoofBuffer *buffer, gpointer output_buff
     if (buffer->n_fragments[buffer_id] < buffer->fragments_per_dataset) return FALSE;
 
     memcpy(output_buffer, dataset_buffer, buffer->dataset_size);
+
     buffer->n_fragments[buffer_id] = 0;
     buffer->current_id += 1;