/*
 * ADBMS_Abstraction.c
 *
 *  Created on: 14.07.2022
 *      Author: max
 */
#include "ADBMS_Abstraction.h"
#include "ADBMS_CMD_MAKROS.h"
#include "ADBMS_LL_Driver.h"
#include "config_ADBMS6830.h"
#include <stddef.h>
#include "NTC.h"

#define SWO_LOG_PREFIX "[ADBMS] "
#include "swo_log.h"

static const char* const HAL_Statuses[] = {"HAL_OK", "HAL_ERROR", "HAL_BUSY", "HAL_TIMEOUT"};

#define CHECK_RETURN(x)                                                                                                \
    do {                                                                                                               \
        HAL_StatusTypeDef status = x;                                                                                  \
        if (status != 0) {                                                                                             \
            debug_log(LOG_LEVEL_ERROR, "in %s:%d@%s: %s failed with status %d (%s)", __FILE_NAME__, __LINE__, __func__,\
                      #x, status,                                                                                      \
                      (status < (sizeof(HAL_Statuses) / sizeof(HAL_Statuses[0]))) ? HAL_Statuses[status] : "Unknown"); \
            return status;                                                                                             \
        }                                                                                                              \
    } while (0)

HAL_StatusTypeDef amsReset() {
    amsWakeUp();
    readCMD(SRST, CMD_EMPTY_BUFFER, 0);

    amsWakeUp();

    uint8_t sidbuffer[CMD_BUFFER_SIZE(SID_GROUP_SIZE)] = {};
    if (readCMD(RDSID, sidbuffer, SID_GROUP_SIZE) != HAL_OK) {
        bool nonzero = false;
        for (size_t i = 0; i < N_BMS; i++) {
            if (sidbuffer[BUFFER_BMS_OFFSET(i, SID_GROUP_SIZE)] != 0) {
                nonzero = true;
            }
        }
        if (nonzero) {
            debug_log(LOG_LEVEL_ERROR,
                      "CRC failure when reading BMS IDs, but some IDs are nonzero. --- Intermittent connection?");
        } else {
            debug_log(LOG_LEVEL_ERROR, "CRC failure when reading BMS IDs, all IDs are zero. --- No connection?");
        }
        return HAL_ERROR;
    }

    

    debug_log(LOG_LEVEL_INFO, "BMS reset complete");

    if (DEBUG_CHANNEL_ENABLED(LOG_LEVEL_INFO)) {
        debug_log(LOG_LEVEL_INFO, "Found IDs: ");
        for (size_t i = 0; i < N_BMS; i++) {
            uint64_t id = 0;
            for (size_t j = 0; j < SID_GROUP_SIZE; j++) {
                id |= sidbuffer[BUFFER_BMS_OFFSET(i, SID_GROUP_SIZE) + j] << (j * 8);
            }
            modules[i].bmsID = id;
            debug_log_cont(LOG_LEVEL_INFO, "0x%08lx%08lx ", (uint32_t)(id >> 32), (uint32_t)(id & 0xFFFFFFFF)); //newlib does not support %llu
        }
    }

    mcuDelay(10);
    amsWakeUp();
    amsStopBalancing();
    amsConfigOverUnderVoltage(DEFAULT_OV, DEFAULT_UV);

    uint8_t flagsbuffer[CMD_BUFFER_SIZE(6)] = {[0 ... CMD_BUFFER_SIZE(6) - 1] = 0xFF}; //technically not all 6 bytes are used, but it's easier to just set all to 0xFF
                                                                                       //see page 27 of the datasheet for the memory map

    CHECK_RETURN(writeCMD(CLRFLAG, flagsbuffer, 6)); //clear flags,
    CHECK_RETURN(writeCMD(CLOVUV, flagsbuffer, 6));  //OVUV flags
    CHECK_RETURN(writeCMD(ADCV | ADCV_CONT | ADCV_RD, flagsbuffer, 0)); //start continuous cell voltage measurement with redundancy
    CHECK_RETURN(writeCMD(ADAX | ADAX_CONV_ALL, flagsbuffer, 0)); //start aux measurement

    return HAL_OK;
}

HAL_StatusTypeDef initAMS(SPI_HandleTypeDef* hspi) {
    // pull MSTR High for Microcontroller mode ADBMS6822
    HAL_GPIO_WritePin(MSTR1_GPIO_Port, MSTR1_Pin, GPIO_PIN_SET);
    adbmsDriverInit(hspi);
    return amsReset();
}

HAL_StatusTypeDef amsWakeUp() {
    return __pollCMD(PLADC, 100); //wake up ADBMS6830, wait for T_wake = 200 us (100 cycles at 500 kHz)
}

HAL_StatusTypeDef amsCellMeasurement(Cell_Module (*module)[N_BMS]) {
    #warning check conversion counter to ensure that continuous conversion has not been stopped
    #warning check for OW conditions: ADSV | ADSV_OW_0 / ADSV_OW_1
    return amsReadCellVoltages(module);
}

HAL_StatusTypeDef amsAuxAndStatusMeasurement(Cell_Module (*module)[N_BMS]) {
    uint8_t rxbuf[CMD_BUFFER_SIZE(STATUS_GROUP_C_SIZE)] = {};
    CHECK_RETURN(readCMD(RDSTATC, rxbuf, STATUS_GROUP_C_SIZE));
    for (size_t i = 0; i < N_BMS; i++) {
        size_t offset = BUFFER_BMS_OFFSET(i, STATUS_GROUP_C_SIZE);
        (*module)[i].status.CS_FLT   = rxbuf[offset + 0] | (rxbuf[offset + 1] << 8);
        (*module)[i].status.CCTS     = rxbuf[offset + 2] | (rxbuf[offset + 3] << 8);
        (*module)[i].status.VA_OV    = (rxbuf[offset + 4] >> 7) & 0x01;
        (*module)[i].status.VA_UV    = (rxbuf[offset + 4] >> 6) & 0x01;
        (*module)[i].status.VD_OV    = (rxbuf[offset + 4] >> 5) & 0x01;
        (*module)[i].status.VD_UV    = (rxbuf[offset + 4] >> 4) & 0x01;
        (*module)[i].status.CED      = (rxbuf[offset + 4] >> 3) & 0x01;
        (*module)[i].status.CMED     = (rxbuf[offset + 4] >> 2) & 0x01;
        (*module)[i].status.SED      = (rxbuf[offset + 4] >> 1) & 0x01;
        (*module)[i].status.SMED     = (rxbuf[offset + 4] >> 0) & 0x01;
        (*module)[i].status.VDEL     = (rxbuf[offset + 5] >> 7) & 0x01;
        (*module)[i].status.VDE      = (rxbuf[offset + 5] >> 6) & 0x01;
        (*module)[i].status.COMPARE  = (rxbuf[offset + 5] >> 5) & 0x01;
        (*module)[i].status.SPIFLT   = (rxbuf[offset + 5] >> 4) & 0x01;
        (*module)[i].status.SLEEP    = (rxbuf[offset + 5] >> 3) & 0x01;
        (*module)[i].status.THSD     = (rxbuf[offset + 5] >> 2) & 0x01;
        (*module)[i].status.TMODCHK  = (rxbuf[offset + 5] >> 1) & 0x01;
        (*module)[i].status.OSCCHK   = (rxbuf[offset + 5] >> 0) & 0x01;
    }

    if (pollCMD(PLAUX) == HAL_BUSY) {
        // return HAL_BUSY;
    }

    constexpr size_t auxGroupSizes[] = {AUX_GROUP_A_SIZE, AUX_GROUP_B_SIZE, AUX_GROUP_C_SIZE, AUX_GROUP_D_SIZE};
    constexpr size_t auxVoltagesPerGroup[] = {3, 3, 3, 1}; // Number of voltages in each group
    constexpr uint8_t auxCommands[] = {RDAUXA, RDAUXB, RDAUXC, RDAUXD};

    // Getting auxVoltages from the BMS
    for (size_t group = 0; group < 4; group++) {
        CHECK_RETURN(readCMD(auxCommands[group], rxbuf, auxGroupSizes[group]));
        for (size_t i = 0; i < N_BMS; i++) {
            size_t offset = BUFFER_BMS_OFFSET(i, auxGroupSizes[group]);
            for (size_t j = 0; j < auxVoltagesPerGroup[group]; j++) {
                (*module)[i].auxVoltages[group * 3 + j] = mV_from_ADBMS6830(rxbuf[offset + j * 2] | (rxbuf[offset + j * 2 + 1] << 8));
            }
        }
    }


    CHECK_RETURN(readCMD(RDSTATA, rxbuf, STATUS_GROUP_A_SIZE));
    for (size_t i = 0; i < N_BMS; i++) {
        size_t offset = BUFFER_BMS_OFFSET(i, STATUS_GROUP_A_SIZE);
        (*module)[i].internalDieTemp = (mV_from_ADBMS6830(rxbuf[offset + 2] | (rxbuf[offset + 3] << 8)) / 7.5f) - 273; //datasheet page 72
    }

    CHECK_RETURN(readCMD(RDSTATB, rxbuf, STATUS_GROUP_B_SIZE));
    for (size_t i = 0; i < N_BMS; i++) {
        size_t offset = BUFFER_BMS_OFFSET(i, STATUS_GROUP_B_SIZE);
        (*module)[i].digitalSupplyVoltage = mV_from_ADBMS6830(rxbuf[offset + 0] | (rxbuf[offset + 1] << 8));
        (*module)[i].analogSupplyVoltage = mV_from_ADBMS6830(rxbuf[offset + 2] | (rxbuf[offset + 3] << 8));
        (*module)[i].refVoltage = mV_from_ADBMS6830(rxbuf[offset + 4] | (rxbuf[offset + 5] << 8));
    }

    CHECK_RETURN(writeCMD(ADAX | ADAX_CONV_ALL, rxbuf, 0)); // start aux conversion for next iteration

    return HAL_OK;
}

HAL_StatusTypeDef amsConfigBalancing(const uint32_t channels[static N_BMS], uint8_t dutyCycle) {
    if (dutyCycle > 0x0F) {
        return HAL_ERROR; // only 4 bits are allowed for dutyCycle
    }

    uint8_t rxbufA[CMD_BUFFER_SIZE(PWM_GROUP_A_SIZE)] = {};
    uint8_t rxbufB[CMD_BUFFER_SIZE(PWM_GROUP_B_SIZE)] = {};

    CHECK_RETURN(readCMD(RDPWMA, rxbufA, PWM_GROUP_A_SIZE));
    CHECK_RETURN(readCMD(RDPWMB, rxbufB, PWM_GROUP_B_SIZE));

    for (size_t i = 0; i < N_BMS; i++) {
        size_t offsetA = BUFFER_BMS_OFFSET(i, PWM_GROUP_A_SIZE);
        size_t offsetB = BUFFER_BMS_OFFSET(i, PWM_GROUP_B_SIZE);

        for (size_t c = 0; c < 16; c += 2) {
            uint8_t high = (channels[i] & (1 << (c + 1))) ? (dutyCycle << 4) : 0;
            uint8_t low = (channels[i] & (1 << c)) ? dutyCycle : 0;

            if (c < 12) {
                rxbufA[offsetA + (c / 2)] = high | low;
            } else {
                rxbufB[offsetB + ((c - 12) / 2)] = high | low;
            }
        }

        // log the new PWM settings
        // output is: PWM - [BMS_ID]: [PWM0] ... [PWM16]
        if (DEBUG_CHANNEL_ENABLED(LOG_LEVEL_DEBUG)) {
            debug_log(LOG_LEVEL_DEBUG, "PWM - %d: ", i);
            for (size_t j = 0; j < 6; j++) {
                debug_log_cont(LOG_LEVEL_DEBUG, "%x %x ", rxbufA[offsetA + j] & 0x0F, rxbufA[offsetA + j] >> 4);
            }
            for (size_t j = 0; j < 2; j++) {
                debug_log_cont(LOG_LEVEL_DEBUG, "%x %x ", rxbufB[offsetB + j] & 0x0F, rxbufB[offsetB + j] >> 4);
            }
        }
    }

    CHECK_RETURN(writeCMD(WRPWMA, rxbufA, PWM_GROUP_A_SIZE));
    CHECK_RETURN(writeCMD(WRPWMB, rxbufB, PWM_GROUP_B_SIZE));

    return HAL_OK;
}

HAL_StatusTypeDef amsStartBalancing() { return writeCMD(UNMUTE, CMD_EMPTY_BUFFER, 0); }

HAL_StatusTypeDef amsStopBalancing() { return writeCMD(MUTE, CMD_EMPTY_BUFFER, 0); }

HAL_StatusTypeDef amsSelfTest() { return 0; }

HAL_StatusTypeDef amsConfigOverUnderVoltage(uint16_t overVoltage, uint16_t underVoltage) {
    uint8_t buffer[CMD_BUFFER_SIZE(CFG_GROUP_B_SIZE)] = {};

    debug_log(LOG_LEVEL_INFO, "Configuring OV/UV thresholds to %u mV (%u internal) / %u mV (%u internal)", overVoltage,
              mV_to_ADBMS6830(overVoltage), underVoltage, mV_to_ADBMS6830(underVoltage));

    CHECK_RETURN(readCMD(RDCFGB, buffer, CFG_GROUP_B_SIZE));

    overVoltage = mV_to_ADBMS6830(overVoltage);
    underVoltage = mV_to_ADBMS6830(underVoltage);

    if (underVoltage & 0xF000 || overVoltage & 0xF000) { // only 12 bits allowed
        debug_log(LOG_LEVEL_ERROR, "Invalid OV/UV thresholds: %u mV / %u mV", overVoltage, underVoltage);
        return HAL_ERROR;
    }

    for (size_t i = 0; i < N_BMS; i++) {
        size_t offset = BUFFER_BMS_OFFSET(i, CFG_GROUP_B_SIZE);

        // UV
        buffer[offset + 0] = (uint8_t)(underVoltage & 0xFF);
        buffer[offset + 1] &= 0xF0;
        buffer[offset + 1] |= (uint8_t)((underVoltage >> 8) & 0x0F);

        // OV
        buffer[offset + 1] &= 0x0F;
        buffer[offset + 1] |= (uint8_t)(overVoltage << 4);
        buffer[offset + 2] = (uint8_t)(overVoltage >> 4);
    }

    return writeCMD(WRCFGB, buffer, CFG_GROUP_B_SIZE);
}

HAL_StatusTypeDef amsCheckUnderOverVoltage(Cell_Module (*module)[N_BMS]) {
    uint8_t regbuffer[CMD_BUFFER_SIZE(STATUS_GROUP_D_SIZE)] = {};

    CHECK_RETURN(readCMD(RDSTATD, regbuffer, STATUS_GROUP_D_SIZE));

    for (size_t i = 0; i < N_BMS; i++) {
        size_t offset = BUFFER_BMS_OFFSET(i, STATUS_GROUP_D_SIZE);
        uint32_t ov_uv_data = 0;
        ov_uv_data = (regbuffer[offset + 0] << 0) | (regbuffer[offset + 1] << 8) | (regbuffer[offset + 2] << 16) |
                     (regbuffer[offset + 3] << 24);

        (*module)[i].overVoltage = 0;
        (*module)[i].underVoltage = 0;

        for (size_t j = 0; j < N_CELLS; j++) { // ov/uv flags are 1-bit flags for each cell C0UV, C0OV, C1UV, C1OV, ...
            (*module)[i].underVoltage |= (ov_uv_data >> (j * 2)) & 0x01;
            (*module)[i].overVoltage |= (ov_uv_data >> (j * 2 + 1)) & 0x01;
        }
    }

    uint8_t buffer[CMD_BUFFER_SIZE(6)] = {[0 ... CMD_BUFFER_SIZE(6) - 1] = 0xFF};

    return writeCMD(CLOVUV, buffer, STATUS_GROUP_D_SIZE); // flags are latched, so we need to clear them
}

HAL_StatusTypeDef amsClearFlag() {
    uint8_t buffer[CMD_BUFFER_SIZE(6)] = {[0 ... CMD_BUFFER_SIZE(6) - 1] = 0xFF};
    return writeCMD(CLRFLAG, buffer, 6);
}

HAL_StatusTypeDef amsClearAux() { return writeCMD(CLRAUX, CMD_EMPTY_BUFFER, 0); }

HAL_StatusTypeDef amsClearOVUV() { 
    uint8_t buffer[CMD_BUFFER_SIZE(6)] = {[0 ... CMD_BUFFER_SIZE(6) - 1] = 0xFF};
    return writeCMD(CLOVUV, buffer, 6);
}

HAL_StatusTypeDef amsReadCellVoltages(Cell_Module (*module)[N_BMS]) {
    uint8_t rxbuffer[CMD_BUFFER_SIZE(CV_GROUP_A_SIZE)] = {};

    CHECK_RETURN(readCMD(RDCVA, rxbuffer, CV_GROUP_A_SIZE));
    for (size_t i = 0; i < N_BMS; i++) {
        size_t offset = BUFFER_BMS_OFFSET(i, CV_GROUP_A_SIZE);
        (*module)[i].cellVoltages[0] = mV_from_ADBMS6830(rxbuffer[offset + 0] | (rxbuffer[offset + 1] << 8));
        (*module)[i].cellVoltages[1] = mV_from_ADBMS6830(rxbuffer[offset + 2] | (rxbuffer[offset + 3] << 8));
        (*module)[i].cellVoltages[2] = mV_from_ADBMS6830(rxbuffer[offset + 4] | (rxbuffer[offset + 5] << 8));
    }

    CHECK_RETURN(readCMD(RDCVB, rxbuffer, CV_GROUP_A_SIZE));
    for (size_t i = 0; i < N_BMS; i++) {
        size_t offset = BUFFER_BMS_OFFSET(i, CV_GROUP_A_SIZE);
        (*module)[i].cellVoltages[3] = mV_from_ADBMS6830(rxbuffer[offset + 0] | (rxbuffer[offset + 1] << 8));
        (*module)[i].cellVoltages[4] = mV_from_ADBMS6830(rxbuffer[offset + 2] | (rxbuffer[offset + 3] << 8));
        (*module)[i].cellVoltages[5] = mV_from_ADBMS6830(rxbuffer[offset + 4] | (rxbuffer[offset + 5] << 8));
    }

    CHECK_RETURN(readCMD(RDCVC, rxbuffer, CV_GROUP_A_SIZE));
    for (size_t i = 0; i < N_BMS; i++) {
        size_t offset = BUFFER_BMS_OFFSET(i, CV_GROUP_A_SIZE);
        (*module)[i].cellVoltages[6] = mV_from_ADBMS6830(rxbuffer[offset + 0] | (rxbuffer[offset + 1] << 8));
        (*module)[i].cellVoltages[7] = mV_from_ADBMS6830(rxbuffer[offset + 2] | (rxbuffer[offset + 3] << 8));
        (*module)[i].cellVoltages[8] = mV_from_ADBMS6830(rxbuffer[offset + 4] | (rxbuffer[offset + 5] << 8));
    }

    CHECK_RETURN(readCMD(RDCVD, rxbuffer, CV_GROUP_A_SIZE));
    for (size_t i = 0; i < N_BMS; i++) {
        size_t offset = BUFFER_BMS_OFFSET(i, CV_GROUP_A_SIZE);
        (*module)[i].cellVoltages[9] = mV_from_ADBMS6830(rxbuffer[offset + 0] | (rxbuffer[offset + 1] << 8));
        (*module)[i].cellVoltages[10] = mV_from_ADBMS6830(rxbuffer[offset + 2] | (rxbuffer[offset + 3] << 8));
        (*module)[i].cellVoltages[11] = mV_from_ADBMS6830(rxbuffer[offset + 4] | (rxbuffer[offset + 5] << 8));
    }

    CHECK_RETURN(readCMD(RDCVE, rxbuffer, CV_GROUP_A_SIZE));
    for (size_t i = 0; i < N_BMS; i++) {
        size_t offset = BUFFER_BMS_OFFSET(i, CV_GROUP_A_SIZE);
        (*module)[i].cellVoltages[12] = mV_from_ADBMS6830(rxbuffer[offset + 0] | (rxbuffer[offset + 1] << 8));
        (*module)[i].cellVoltages[13] = mV_from_ADBMS6830(rxbuffer[offset + 2] | (rxbuffer[offset + 3] << 8));
        (*module)[i].cellVoltages[14] = mV_from_ADBMS6830(rxbuffer[offset + 4] | (rxbuffer[offset + 5] << 8));
    }

    CHECK_RETURN(readCMD(RDCVF, rxbuffer, CV_GROUP_A_SIZE));
    for (size_t i = 0; i < N_BMS; i++) {
        size_t offset = BUFFER_BMS_OFFSET(i, CV_GROUP_A_SIZE);
        (*module)[i].cellVoltages[15] = mV_from_ADBMS6830(rxbuffer[offset + 0] | (rxbuffer[offset + 1] << 8));
    }

    return HAL_OK;
}

// Each selected BMS must have a corresponding address, and the data array for that BMS must be at least datalens[i]
// bytes long
HAL_StatusTypeDef amsSendI2C(const uint8_t addresses[static N_BMS], uint8_t* data[static N_BMS],
                             const uint8_t datalens[static N_BMS], uint32_t bms_mask) {
    uint8_t buffer[CMD_BUFFER_SIZE(COMM_GROUP_SIZE)] = {};

    // COMM register works in 3 bytes max per go, interleaved with control information

    uint8_t max_datalen = 0;
    for (size_t i = 0; i < N_BMS; i++) {
        if (datalens[i] > max_datalen) {
            max_datalen = datalens[i];
        }
    }

    for (size_t i = 0; i < N_BMS; i++) {
        size_t offset = BUFFER_BMS_OFFSET(i, COMM_GROUP_SIZE);

        if (!(bms_mask & (1 << i))) {
            buffer[offset + 0] = I2C_SEND_NOTRANSFER;
            buffer[offset + 2] = I2C_SEND_NOTRANSFER;
            buffer[offset + 4] = I2C_SEND_NOTRANSFER;
            continue;
        }
    }

    size_t packet_count =
        ((max_datalen + 1) / 3) + ((max_datalen + 1) % 3 != 0); // number of 3 byte packets needed to send all data

  for (size_t i = 0; i < (packet_count * 3); i++) { //i - 1 is the number of data bytes sent so far (1 byte for the address)
        for (size_t j = 0; j < N_BMS; j++) {
            size_t offset = BUFFER_BMS_OFFSET(j, COMM_GROUP_SIZE);
            if (!(bms_mask & (1 << j)))
                continue; // skip BMS that are not selected

            if (i == 0) {
                buffer[offset + 0] = I2C_SEND_START;
                buffer[offset + 1] = addresses[j] << 1; // shift the address left by 1 to make space for the R/W bit
                continue;
            }

            // add data to the buffer

            // case 1: the last group of 3 bytes contained the last data byte, so we need to send a stop
            if (datalens[j] - (int)i == -1) {
                buffer[offset + ((i % 3) * 2)] = I2C_SEND_STOP;
                continue;
            }

            // case 2: we are more than one byte past the end, so we don't need to send any data
            if (datalens[j] - (int)i < -1) {
                buffer[offset + ((i % 3) * 2)] = I2C_SEND_NOTRANSFER;
                continue;
            }

            // case 3: we are still sending data
            buffer[offset + ((i % 3) * 2)] = I2C_SEND;
            buffer[offset + ((i % 3) * 2) + 1] = data[j][i - 1];
        }

        // send the data
        if (i % 3 == 0 && i != 0) {
            CHECK_RETURN(writeCMD(WRCOMM, buffer, COMM_GROUP_SIZE));

            __pollCMD(STCOMM, 72); // wait 72 cycles for the I2C transfer to complete (datasheet page 43)
                                   // TODO: not sure if this is correct for a daisy chain
        }
    }

    // send the last packet
    CHECK_RETURN(writeCMD(WRCOMM, buffer, COMM_GROUP_SIZE));
    __pollCMD(STCOMM, 72);

    return HAL_OK;
}

// Each selected BMS must have a corresponding address, and the data array for that BMS must be at least datalens[i]
// bytes long
HAL_StatusTypeDef amsReadI2C(const uint8_t addresses[static N_BMS], uint8_t* data[static N_BMS],
                             const uint8_t datalens[static N_BMS], uint32_t bms_mask) {
    uint8_t buffer[CMD_BUFFER_SIZE(COMM_GROUP_SIZE)] = {};

    uint8_t max_datalen = 0;
    for (size_t i = 0; i < N_BMS; i++) {
        if (datalens[i] > max_datalen) {
            max_datalen = datalens[i];
        }
    }

    for (size_t i = 0; i < N_BMS; i++) {
        size_t offset = BUFFER_BMS_OFFSET(i, COMM_GROUP_SIZE);

        if (!(bms_mask & (1 << i))) {
            buffer[offset + 0] = I2C_SEND_NOTRANSFER;
            buffer[offset + 2] = I2C_SEND_NOTRANSFER;
            buffer[offset + 4] = I2C_SEND_NOTRANSFER;
            continue;
        }
    }

    size_t packet_count = ((max_datalen + 1) / 3) + ((max_datalen + 1) % 3 != 0);

    for (size_t i = 0; i < (packet_count * 3);
         i++) { // i - 1 is the number of data bytes sent so far (1 byte for the address)
        for (size_t j = 0; j < N_BMS; j++) {
            size_t offset = BUFFER_BMS_OFFSET(j, COMM_GROUP_SIZE);
            if (!(bms_mask & (1 << j)))
                continue; // skip BMS that are not selected

            if (i == 0) {
                buffer[offset + 0] = I2C_SEND_START;
              buffer[offset + 1] = addresses[j] << 1 | 0x01; // shift the address left by 1 to make space for the R/W bit
                continue;
            }

            // add data to the buffer

            // case 1: the last group of 3 bytes contained the last data byte, so we need to send a stop
            if (datalens[j] - (int)i == -1) {
                buffer[offset + ((i % 3) * 2)] = I2C_SEND_STOP;
                continue;
            }

            // case 2: we are more than one byte past the end, so we don't need to receive any data
            if (datalens[j] - (int)i < -1) {
                buffer[offset + ((i % 3) * 2)] = I2C_SEND_NOTRANSFER;
                continue;
            }

            // case 3: we are still receiving data
            buffer[offset + ((i % 3) * 2)] = I2C_SEND_ACK;
        }

        // send the command data
        if (i % 3 == 0 && i != 0) {
            CHECK_RETURN(writeCMD(WRCOMM, buffer, COMM_GROUP_SIZE));

            __pollCMD(STCOMM, 72); // wait 72 cycles for the I2C transfer to complete (datasheet page 43)
                                   // TODO: not sure if this is correct for a daisy chain

            // read the data
            CHECK_RETURN(readCMD(RDCOMM, buffer, COMM_GROUP_SIZE));

            for (size_t j = 0; j < N_BMS; j++) {
                size_t offset = BUFFER_BMS_OFFSET(j, COMM_GROUP_SIZE);

                if (!(bms_mask & (1 << j)))
                    continue; // skip BMS that are not selected

                for (size_t k = 0; k < 3; k++) {
                    if (datalens[j] - (int)i < -1) {
                        continue;
                    }

                    data[j][i - 1 + k] = buffer[offset + (k * 2) + 1];
                }
            }
        }
    }

    // send the last packet
    CHECK_RETURN(writeCMD(WRCOMM, buffer, COMM_GROUP_SIZE));
    __pollCMD(STCOMM, 72);

    // read the data
    CHECK_RETURN(readCMD(RDCOMM, buffer, COMM_GROUP_SIZE));

    for (size_t j = 0; j < N_BMS; j++) {
        size_t offset = BUFFER_BMS_OFFSET(j, COMM_GROUP_SIZE);

        if (!(bms_mask & (1 << j)))
            continue; // skip BMS that are not selected

        for (size_t k = 0; k < 3; k++) {
            if (datalens[j] - (int)max_datalen < -1) {
                continue;
            }

            data[j][max_datalen - 1 + k] = buffer[offset + (k * 2) + 1];
        }
    }

    return HAL_OK;
}