#include "radar_hdf5.h"

//* HDF5 pointers
hid_t file_id, group_scans_a, group_scans_b;
hid_t group_ahrs, group_reports_a, group_reports_b;

//* Constants
enum Channel
{
    CH_A = 1,
    CH_B
};

//* prototypes
void h5_malloc_frames(int ch, int dim);
void h5_malloc_reports(int ch, int dim);
void h5_write_scans(int ch, int dim);
void h5_write_reports(int ch, int dim);
void h5_copy_frame(int ch, int pos, uint8_t data[SCANS_PER_FRAME][SPOKELEN], double times[SCANS_PER_FRAME], double angles[SCANS_PER_FRAME], int ranges[SCANS_PER_FRAME]);
void h5_copy_report(int ch, int pos, double *time, struct RadarReport_02C4_99 *report);

//* datasetID names
//  GROUPS
#define _GROUP_SCANS_A "/radar_a"
#define _GROUP_SCANS_B "/radar_b"
#define _GROUP_AHRS "/ahrs"
#define _GROUP_REPORTS_A "/reports_a"
#define _GROUP_REPORTS_B "/reports_b"

void h5_open(const char *fname)
{
    file_id = H5Fcreate(fname, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);

    //* Create Groups
    group_scans_a = H5Gcreate2(file_id, _GROUP_SCANS_A, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    group_scans_b = H5Gcreate2(file_id, _GROUP_SCANS_B, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    group_reports_a = H5Gcreate2(file_id, _GROUP_REPORTS_A, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    group_reports_b = H5Gcreate2(file_id, _GROUP_REPORTS_B, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    group_ahrs = H5Gcreate2(file_id, _GROUP_AHRS, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
}

void h5_close()
{
    H5Gclose(group_scans_a);
    H5Gclose(group_scans_b);
    H5Gclose(group_reports_a);
    H5Gclose(group_reports_b);
    H5Gclose(group_ahrs);
    H5Fclose(file_id);
}

void h5_write_scans(int ch, int dim)
{
    struct Scans *s;
    if (ch == CH_A)
        s = &scans_a;
    else
        s = &scans_b;

    //* Check if the memory is allocated
    if (s->n_samples == 0)
        return;

    // * Identifiers
    hsize_t dims[2];
    hid_t dataset_id, dataspace_id;

    const char *ds_name_scan, *ds_name_angle, *ds_name_time, *ds_name_range;
    if (ch == CH_A)
    {
        ds_name_scan = _GROUP_SCANS_A "/scan";
        ds_name_time = _GROUP_SCANS_A "/time";
        ds_name_angle = _GROUP_SCANS_A "/angle";
        ds_name_range = _GROUP_SCANS_A "/range";
    }
    else
    {
        ds_name_scan = _GROUP_SCANS_B "/scan";
        ds_name_time = _GROUP_SCANS_B "/time";
        ds_name_angle = _GROUP_SCANS_B "/angle";
        ds_name_range = _GROUP_SCANS_B "/range";
    }

    // intensity
    dims[0] = dim;
    dims[1] = SPOKELEN;
    dataspace_id = H5Screate_simple(2, dims, NULL);
    dataset_id = H5Dcreate2(file_id, ds_name_scan, H5T_STD_U8BE, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_STD_U8BE, H5S_ALL, H5S_ALL, H5P_DEFAULT, s->data);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);
    // angles
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, ds_name_angle, H5T_NATIVE_DOUBLE, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_DOUBLE, H5S_ALL, H5S_ALL, H5P_DEFAULT, s->angles);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);
    // times
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, ds_name_time, H5T_NATIVE_DOUBLE, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_DOUBLE, H5S_ALL, H5S_ALL, H5P_DEFAULT, s->times);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);
    // ranges
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, ds_name_range, H5T_NATIVE_INT, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_INT, H5S_ALL, H5S_ALL, H5P_DEFAULT, s->ranges);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);
}
void h5_write_scans_a(int dim) { h5_write_scans(CH_A, dim); }
void h5_write_scans_b(int dim) { h5_write_scans(CH_B, dim); }

void h5_write_reports(int ch, int dim)
{

    struct Reports *r;
    //* Select channel
    r = (ch == CH_A) ? &reports_a : &reports_b;

    //* Check if the memory is allocated
    if (r->n_samples == 0)
        return;

    // * Identifiers
    hsize_t dims[1];
    dims[0] = dim;
    hid_t dataset_id, dataspace_id;

    const char *ds_name_time, *ds_name_command, *ds_name_range, *ds_name_gain;
    const char *ds_name_sea_auto, *ds_name_sea, *ds_name_rain, *ds_name_interference_rejection;
    const char *ds_name_target_expansion, *ds_name_target_boost;
    if (ch == CH_A)
    {
        ds_name_time = _GROUP_REPORTS_A "/time";
        ds_name_command = _GROUP_REPORTS_A "/command";
        ds_name_range = _GROUP_REPORTS_A "/range";
        ds_name_gain = _GROUP_REPORTS_A "/gain";
        ds_name_sea_auto = _GROUP_REPORTS_A "/sea_auto";
        ds_name_sea = _GROUP_REPORTS_A "/sea";
        ds_name_rain = _GROUP_REPORTS_A "/rain";
        ds_name_interference_rejection = _GROUP_REPORTS_A "/interference_rejection";
        ds_name_target_expansion = _GROUP_REPORTS_A "/target_expansion";
        ds_name_target_boost = _GROUP_REPORTS_A "/target_boost";
    }
    else
    {
        ds_name_time = _GROUP_REPORTS_B "/time";
        ds_name_command = _GROUP_REPORTS_B "/command";
        ds_name_range = _GROUP_REPORTS_B "/range";
        ds_name_gain = _GROUP_REPORTS_B "/gain";
        ds_name_sea_auto = _GROUP_REPORTS_B "/sea_auto";
        ds_name_sea = _GROUP_REPORTS_B "/sea";
        ds_name_rain = _GROUP_REPORTS_B "/rain";
        ds_name_interference_rejection = _GROUP_REPORTS_B "/interference_rejection";
        ds_name_target_expansion = _GROUP_REPORTS_B "/target_expansion";
        ds_name_target_boost = _GROUP_REPORTS_B "/target_boost";
    }

    // times
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, ds_name_time, H5T_NATIVE_DOUBLE, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_DOUBLE, H5S_ALL, H5S_ALL, H5P_DEFAULT, r->times);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // command
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, ds_name_command, H5T_NATIVE_UINT8, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_UINT8, H5S_ALL, H5S_ALL, H5P_DEFAULT, r->command);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // range
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, ds_name_range, H5T_NATIVE_UINT32, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_UINT32, H5S_ALL, H5S_ALL, H5P_DEFAULT, r->range);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // gain
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, ds_name_gain, H5T_NATIVE_UINT8, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_UINT8, H5S_ALL, H5S_ALL, H5P_DEFAULT, r->gain);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // sea_auto
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, ds_name_sea_auto, H5T_NATIVE_UINT8, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_UINT8, H5S_ALL, H5S_ALL, H5P_DEFAULT, r->sea_auto);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // sea
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, ds_name_sea, H5T_NATIVE_UINT32, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_UINT32, H5S_ALL, H5S_ALL, H5P_DEFAULT, r->sea);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // rain
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, ds_name_rain, H5T_NATIVE_UINT8, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_UINT8, H5S_ALL, H5S_ALL, H5P_DEFAULT, r->rain);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // interference_rejection
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, ds_name_interference_rejection, H5T_NATIVE_UINT8, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_UINT8, H5S_ALL, H5S_ALL, H5P_DEFAULT, r->interference_rejection);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // target_expansion
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, ds_name_target_expansion, H5T_NATIVE_UINT8, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_UINT8, H5S_ALL, H5S_ALL, H5P_DEFAULT, r->target_expansion);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // target_boost
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, ds_name_target_boost, H5T_NATIVE_UINT8, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_UINT8, H5S_ALL, H5S_ALL, H5P_DEFAULT, r->target_boost);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);
}
void h5_write_reports_a(int dim) { h5_write_reports(CH_A, dim); }
void h5_write_reports_b(int dim) { h5_write_reports(CH_B, dim); }

void h5_write_ahrs()
{

    // * Identifiers
    hsize_t dims[1];
    dims[0] = ahrs.n_samples;
    hid_t dataset_id, dataspace_id;

    // packet_time
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/packet_time", H5T_NATIVE_DOUBLE, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_DOUBLE, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.packet_time);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // lcm timestamp
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/timestamp", H5T_NATIVE_DOUBLE, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_DOUBLE, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.timestamp);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // latitude
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/latitude", H5T_NATIVE_FLOAT, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.latitude);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // longitude
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/longitude", H5T_NATIVE_FLOAT, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.longitude);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // yawAngleTrue
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/yawAngleTrue", H5T_NATIVE_FLOAT, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.yawAngleTrue);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // pitchAngle
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/pitchAngle", H5T_NATIVE_FLOAT, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.pitchAngle);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // rollAngle
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/rollAngle", H5T_NATIVE_FLOAT, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.rollAngle);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // xAccel
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/xAccel", H5T_NATIVE_FLOAT, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.xAccel);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // yAccel
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/yAccel", H5T_NATIVE_FLOAT, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.yAccel);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // zAccel
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/zAccel", H5T_NATIVE_FLOAT, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.zAccel);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // nVelocity
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/nVelocity", H5T_NATIVE_FLOAT, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.nVelocity);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // eVelocity
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/eVelocity", H5T_NATIVE_FLOAT, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.eVelocity);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // dVelocity
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/dVelocity", H5T_NATIVE_FLOAT, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.dVelocity);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // altitude
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/altitude", H5T_NATIVE_FLOAT, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.altitude);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // xRateCorrected
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/xRateCorrected", H5T_NATIVE_FLOAT, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.xRateCorrected);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // yRateCorrected
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/yRateCorrected", H5T_NATIVE_FLOAT, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.yRateCorrected);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // zRateCorrected
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/zRateCorrected", H5T_NATIVE_FLOAT, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.zRateCorrected);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);

    // xRateTemp
    dataspace_id = H5Screate_simple(1, dims, NULL);
    dataset_id = H5Dcreate2(file_id, "ahrs/xRateTemp", H5T_NATIVE_FLOAT, dataspace_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    H5Dwrite(dataset_id, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, ahrs.xRateTemp);
    H5Dclose(dataset_id);
    H5Sclose(dataspace_id);
}

void h5_malloc_frames(int ch, int dim)
{

    struct Scans *f;
    // Select channel
    f = (ch == CH_A) ? &scans_a : &scans_b;

    f->n_samples = dim;
    f->times = calloc(f->n_samples, sizeof(double));
    f->angles = calloc(f->n_samples, sizeof(double));
    f->ranges = calloc(f->n_samples, sizeof(int));
    f->data = malloc(f->n_samples * SPOKELEN * sizeof(uint8_t));
}
void h5_malloc_scans_a(int dim) { h5_malloc_frames(CH_A, dim); }
void h5_malloc_scans_b(int dim) { h5_malloc_frames(CH_B, dim); }

void h5_malloc_reports(int ch, int dim)
{

    struct Reports *r;
    // Select channel
    r = (ch == CH_A) ? &reports_a : &reports_b;

    r->n_samples = dim;
    r->times = calloc(r->n_samples, sizeof(double));
    r->command = calloc(r->n_samples, sizeof(uint8_t)); // placeholder
    r->range = calloc(r->n_samples, sizeof(uint32_t));
    r->gain = calloc(r->n_samples, sizeof(uint8_t));     // placeholder
    r->sea_auto = calloc(r->n_samples, sizeof(uint8_t)); // 0 = off, 1 = harbour, 2 = offshore
    r->sea = calloc(r->n_samples, sizeof(uint32_t));     // sea state
    r->rain = calloc(r->n_samples, sizeof(uint8_t));     // rain clutter
    r->interference_rejection = calloc(r->n_samples, sizeof(uint8_t));
    r->target_expansion = calloc(r->n_samples, sizeof(uint8_t));
    r->target_boost = calloc(r->n_samples, sizeof(uint8_t));
}
void h5_malloc_reports_a(int dim) { h5_malloc_reports(CH_A, dim); }
void h5_malloc_reports_b(int dim) { h5_malloc_reports(CH_B, dim); }

void h5_malloc_ahrs(int dim)
{
    ahrs.n_samples = dim;
    ahrs.packet_time = calloc(dim, sizeof(double));
    ahrs.timestamp = calloc(dim, sizeof(double));
    ahrs.latitude = calloc(dim, sizeof(float));
    ahrs.longitude = calloc(dim, sizeof(float));
    ahrs.yawAngleTrue = calloc(dim, sizeof(float));
    ahrs.pitchAngle = calloc(dim, sizeof(float));
    ahrs.rollAngle = calloc(dim, sizeof(float));
    ahrs.xAccel = calloc(dim, sizeof(float));
    ahrs.yAccel = calloc(dim, sizeof(float));
    ahrs.zAccel = calloc(dim, sizeof(float));
    ahrs.nVelocity = calloc(dim, sizeof(float));
    ahrs.eVelocity = calloc(dim, sizeof(float));
    ahrs.dVelocity = calloc(dim, sizeof(float));
    ahrs.altitude = calloc(dim, sizeof(float));
    ahrs.xRateCorrected = calloc(dim, sizeof(float));
    ahrs.yRateCorrected = calloc(dim, sizeof(float));
    ahrs.zRateCorrected = calloc(dim, sizeof(float));
    ahrs.xRateTemp = calloc(dim, sizeof(float));
}

void h5_copy_ahrs(int pos, double *time, lcmtypes_wec_ahrs_data_l *message)
{
    //* check if malloc_ahrs called
    if (ahrs.n_samples == 0)
        return;

    //* insert data
    ahrs.packet_time[pos] = *time;
    ahrs.timestamp[pos] = message->timestamp;
    ahrs.latitude[pos] = message->latitude;
    ahrs.longitude[pos] = message->longitude;
    ahrs.yawAngleTrue[pos] = message->yawAngleTrue;
    ahrs.pitchAngle[pos] = message->pitchAngle;
    ahrs.rollAngle[pos] = message->rollAngle;
    ahrs.xAccel[pos] = message->xAccel;
    ahrs.yAccel[pos] = message->yAccel;
    ahrs.zAccel[pos] = message->zAccel;
    ahrs.nVelocity[pos] = message->nVelocity;
    ahrs.eVelocity[pos] = message->eVelocity;
    ahrs.dVelocity[pos] = message->dVelocity;
    ahrs.altitude[pos] = message->altitude;
    ahrs.xRateCorrected[pos] = message->xRateCorrected;
    ahrs.yRateCorrected[pos] = message->yRateCorrected;
    ahrs.zRateCorrected[pos] = message->zRateCorrected;
    ahrs.xRateTemp[pos] = message->xRateTemp;
}

void h5_copy_report(int ch, int pos, double *time, struct RadarReport_02C4_99 *report)
{

    struct Reports *r;
    //* Select channel
    r = (ch == CH_A) ? &reports_a : &reports_b;

    //* Check if the memory is allocated
    if (r->n_samples == 0)
        return;

    //* Copy values
    r->times[pos] = *time;
    r->command[pos] = report->command;
    r->range[pos] = report->range;
    r->gain[pos] = report->gain;
    r->sea_auto[pos] = report->sea_auto; // 0 = off, 1 = harbour, 2 = offshore
    r->sea[pos] = report->sea;           // sea state
    r->rain[pos] = report->rain;         // rain clutter
    r->interference_rejection[pos] = report->interference_rejection;
    r->target_expansion[pos] = report->target_expansion;
    r->target_boost[pos] = report->target_boost;
}
void h5_copy_report_a(int pos, double *time, struct RadarReport_02C4_99 *report) { h5_copy_report(CH_A, pos, time, report); }
void h5_copy_report_b(int pos, double *time, struct RadarReport_02C4_99 *report) { h5_copy_report(CH_B, pos, time, report); }

void h5_copy_frame(int ch, int pos, uint8_t data[SCANS_PER_FRAME][SPOKELEN], double times[SCANS_PER_FRAME], double angles[SCANS_PER_FRAME], int ranges[SCANS_PER_FRAME])
{
    pos *= SCANS_PER_FRAME;

    struct Scans *s;

    if (ch == CH_A)
        s = &scans_a;
    else
        s = &scans_b;

    //* Check if the memory is allocated
    if (s->n_samples == 0)
        return;

    //* Copy the single row arrays first
    uint64_t i, j, dbl_pos, int_pos;
    static const size_t dbl_sz = SCANS_PER_FRAME * sizeof(double);
    static const size_t int_sz = SCANS_PER_FRAME * sizeof(int);
    memcpy(&(s->times[pos]), times, dbl_sz);
    memcpy(&(s->angles[pos]), angles, dbl_sz);
    memcpy(&(s->ranges[pos]), ranges, int_sz);

    const size_t uint8_sz = SPOKELEN * sizeof(uint8_t);
    for (i = 0; i < SCANS_PER_FRAME; ++i)
    {
        memcpy(&(s->data[pos * SPOKELEN + i * SPOKELEN]), &data[i], uint8_sz);
    }
}
void h5_copy_frame_a(int pos, uint8_t data[SCANS_PER_FRAME][SPOKELEN], double times[SCANS_PER_FRAME], double angles[SCANS_PER_FRAME], int ranges[SCANS_PER_FRAME])
{
    h5_copy_frame(CH_A, pos, data, times, angles, ranges);
}
void h5_copy_frame_b(int pos, uint8_t data[SCANS_PER_FRAME][SPOKELEN], double times[SCANS_PER_FRAME], double angles[SCANS_PER_FRAME], int ranges[SCANS_PER_FRAME])
{
    h5_copy_frame(CH_B, pos, data, times, angles, ranges);
}

void h5_free_all()
{
    if (scans_a.n_samples != 0)
    {
        free(scans_a.angles);
        free(scans_a.times);
        free(scans_a.ranges);
        free(scans_a.data);
    }
    if (scans_b.n_samples != 0)
    {
        free(scans_b.angles);
        free(scans_b.times);
        free(scans_b.ranges);
        free(scans_b.data);
    }
    if (reports_a.n_samples != 0)
    {
        free(reports_a.times);   // time array
        free(reports_a.command); // placeholder
        free(reports_a.range);
        free(reports_a.gain);     // placeholder
        free(reports_a.sea_auto); // 0 = off, 1 = harbour, 2 = offshore
        free(reports_a.sea);      // sea state
        free(reports_a.rain);     // rain clutter
        free(reports_a.interference_rejection);
        free(reports_a.target_expansion);
        free(reports_a.target_boost);
    }
    if (reports_b.n_samples != 0)
    {
        free(reports_b.times);   // time array
        free(reports_b.command); // placeholder
        free(reports_b.range);
        free(reports_b.gain);     // placeholder
        free(reports_b.sea_auto); // 0 = off, 1 = harbour, 2 = offshore
        free(reports_b.sea);      // sea state
        free(reports_b.rain);     // rain clutter
        free(reports_b.interference_rejection);
        free(reports_b.target_expansion);
        free(reports_b.target_boost);
    }
}