/*
 * AMS_HighLevel.c
 *
 *  Created on: 20.07.2022
 *      Author: max
 */

#include "ADBMS_HighLevel.h"
#include "ADBMS_Abstraction.h"
#include "ADBMS_Driver.h"
#include "config_ADBMS6830.h"
#include "stm32h7xx_hal.h"
#include <stdint.h>
#include <string.h>

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

Cell_Module modules[N_BMS] = {};
uint32_t balancedCells = 0;
bool balancingActive = false;

uint8_t packetChecksumFails = 0;
#define MAX_PACKET_CHECKSUM_FAILS 5

uint8_t deviceSleeps = 0;
#define MAX_DEVICE_SLEEP 3 // TODO: change to correct value

struct pollingTimes {
    uint32_t S_ADC_OW_CHECK;
    uint32_t TMP1075;
};

struct pollingTimes pollingTimes = {0, 0};

static constexpr ADBMS_DetailedStatus NO_ERROR = {ADBMS_NO_ERROR};

ADBMS_DetailedStatus AMS_Init(SPI_HandleTypeDef* hspi) {
    debug_log(LOG_LEVEL_INFO, "ADBMS6830B HAL - configured for %d controllers and %d cells per controller...", N_BMS,
              N_CELLS);
    if (initAMS(hspi) != HAL_OK) {
        debug_log(LOG_LEVEL_ERROR, "ADBMS6830B HAL - initialization failed");
        return (ADBMS_DetailedStatus){ADBMS_INTERNAL_BMS_FAULT, -1};
    }

    pollingTimes = (struct pollingTimes){HAL_GetTick(), HAL_GetTick()};
    packetChecksumFails = 0;
    deviceSleeps = 0;
    return NO_ERROR;
}

#define any(x)                                                                                                         \
    ({                                                                                                                 \
        int first_match = -1;                                                                                          \
        for (size_t __any_intern_i = 0; __any_intern_i < N_BMS; __any_intern_i++) {                                    \
            Cell_Module module = modules[__any_intern_i];                                                              \
            if ((x)) {                                                                                                 \
                first_match = __any_intern_i;                                                                          \
                break;                                                                                                 \
            }                                                                                                          \
        }                                                                                                              \
        first_match + 1;                                                                                               \
    })

ADBMS_DetailedStatus AMS_Idle_Loop() {
    if (!amsWakeUp()) {
        // error_data.data_kind = SEK_INTERNAL_BMS_TIMEOUT; //we don't receive data for the wakeup command
        // set_error_source(ERROR_SOURCE_INTERNAL);         //so we can't tell if we timed out
    }

    packetChecksumFails += (amsAuxAndStatusMeasurement(&modules) == HAL_ERROR);

    int match = 0;
    if ((match = any(module.status.SLEEP))) {
        deviceSleeps++;
        if (deviceSleeps > MAX_DEVICE_SLEEP) {
            return (ADBMS_DetailedStatus){ADBMS_INTERNAL_BMS_TIMEOUT, match - 1};
        } else {
            debug_log(LOG_LEVEL_WARNING, "BMS %d unexpectedly in sleep mode, resetting...", match - 1);
            amsReset();
        }
    }

    if ((match = any(module.status.CS_FLT || module.status.SPIFLT || module.status.CMED || module.status.SMED ||
            module.status.VDE || module.status.VDEL || module.status.OSCCHK || module.status.TMODCHK))) {
        // Fault bits are latched -- clear them so we can check again next
        // iteration.
        amsClearFlag();
        // Log specific BMS fault details
        debug_log(LOG_LEVEL_ERROR, "Fault on BMS %d: ", match - 1);
        auto faultyModule = &modules[match - 1];
        if (faultyModule->status.CS_FLT)    debug_log_cont(LOG_LEVEL_ERROR, "CS_FLT ");
        if (faultyModule->status.SPIFLT)    debug_log_cont(LOG_LEVEL_ERROR, "SPIFLT ");
        if (faultyModule->status.CMED)      debug_log_cont(LOG_LEVEL_ERROR, "CMED ");
        if (faultyModule->status.SMED)      debug_log_cont(LOG_LEVEL_ERROR, "SMED ");
        if (faultyModule->status.VDE)       debug_log_cont(LOG_LEVEL_ERROR, "VDE ");
        if (faultyModule->status.VDEL)      debug_log_cont(LOG_LEVEL_ERROR, "VDEL ");
        if (faultyModule->status.OSCCHK)    debug_log_cont(LOG_LEVEL_ERROR, "OSCCHK ");
        if (faultyModule->status.TMODCHK)   debug_log_cont(LOG_LEVEL_ERROR, "TMODCHK ");

        return (ADBMS_DetailedStatus){ADBMS_INTERNAL_BMS_FAULT, match - 1};
    }

    packetChecksumFails += (amsCellMeasurement(&modules) == HAL_ERROR);
    packetChecksumFails += (amsCheckUnderOverVoltage(&modules) == HAL_ERROR);

    if (packetChecksumFails > MAX_PACKET_CHECKSUM_FAILS) {
        return (ADBMS_DetailedStatus){ADBMS_INTERNAL_BMS_CHECKSUM_FAIL, -1};
    }

    if ((match = any(module.overVoltage))) {
        amsClearOVUV();
        return (ADBMS_DetailedStatus){ADBMS_OVERVOLT, match - 1};
    }

    if ((match = any(module.underVoltage))) {
        amsClearOVUV();
        return (ADBMS_DetailedStatus){ADBMS_UNDERVOLT, match - 1};
    }

    if ((match = any(module.internalDieTemp > ADBMS_MAX_CHIP_TEMP || module.status.THSD))) { 
        return (ADBMS_DetailedStatus){ADBMS_INTERNAL_BMS_OVERTEMP, match - 1};
    }

    // mcuDelay(10);

    return NO_ERROR;
}
