//
// Created by xemon on 23.11.22.
//

#include "CrashDetection.h"
#include "CmsisRtosThreadUtils.h"
#include "SystemDelayInterface.h"

#define LOG_SIGN "Опр.Авар."
#define LOGGER env->logger

void CrashDetection_Init(
        tCrashDetection *env,
        tAccelDataFlowInterface *accelData,
        float *asi15threshold,
        tCarFlipDetectionSettings *settings,
        tLoggerInterface *logger
) {
    env->logger = logger;

    CarCrashDetection_Init(&env->carCrashDetection, accelData, asi15threshold, settings);
    env->emergencyEvents = CarCrashDetection_GetInterface(&env->carCrashDetection);
    env->simulationInterface = CarCrashDetection_GetSimInterface(&env->carCrashDetection);

    InitThreadAtrStatic(
            &env->thread.attr,
            "CrashDetection",
            env->thread.controlBlock,
            env->thread.stack,
            osPriorityNormal
    );

    env->thread.id = 0;
}


static _Noreturn void CrashDetection_Thread(tCrashDetection *env) {
    uint32_t time = SystemGetMs();
    float asi, maxAsi = 0;
    uint32_t count = 0;

    for (;;) {
        CarCrashDetection_Processing(&env->carCrashDetection, SystemWaitForever);
#if false
        asi = EraGlonassAsi15_GetCurrentValue(&env->carCrashDetection.asi15);
        if (maxAsi < asi) {
            maxAsi = asi;
        }
        ++count;

        if (time < SystemGetMs()) {
            LoggerCnInfoStatic(LOGGER, LOG_SIGN, "CCD >")

            LoggerPrintf(LOGGER, " asi<%f> count %u", maxAsi, count);
            maxAsi = 0;
            count = 0;

//            LoggerPrintf(LOGGER, "grav[%f %f %f] cos<%f>",
//                         env->carCrashDetection.flip.current.value.x,
//                         env->carCrashDetection.flip.current.value.y,
//                         env->carCrashDetection.flip.current.value.z,
//                         env->carCrashDetection.flip.current.angleCos
//            );
            LoggerInfoStatic(LOGGER, LOG_SIGN, "<")
            time = SystemGetMs() + 1000;
        }
#endif
    }
}

void CrashDetection_StartThread(tCrashDetection *env) {
    if (!env->thread.id) {
        env->thread.id = osThreadNew((osThreadFunc_t) (CrashDetection_Thread), (void *) (env), &env->thread.attr);
    }
}